static void setupUART(void) {
  HalUARTInit();
  
  // configure UART
  uartConfig.configured           = TRUE;
  uartConfig.baudRate             = HAL_UART_BR_57600;
  uartConfig.flowControl          = HAL_UART_FLOW_OFF;
  uartConfig.flowControlThreshold = 0;
  uartConfig.rx.maxBufSize        = 20;
  uartConfig.tx.maxBufSize        = 128;
  uartConfig.idleTimeout          = 0;         
  uartConfig.intEnable            = TRUE;
  uartConfig.callBackFunc         = (halUARTCBack_t)uartCallback;
  //uartConfig.callBackFunc         = NULL;
  
  //start UART
  //assumes no issues with starting UART
#if (HAL_UART_ISR == 1)
  (void)HalUARTOpen(HAL_UART_PORT_0, &uartConfig);
#else
  (void)HalUARTOpen(HAL_UART_PORT_1, &uartConfig);
#endif
  
  //assumes there is no problem with getting these blocks of bytes
  rxBuffer = osal_mem_alloc(20); //expanded to handle name changes
  modeSelStr = osal_mem_alloc(3);
}

/*********************************************************************
 * @fn      OADTarget_AddService
 *
 * @brief   Initializes the OAD Service by registering GATT attributes
 *          with the GATT server. Only call this function once.
 *
 * @return  The return value of GATTServApp_RegisterForMsg().
 */
bStatus_t OADTarget_AddService(void)
{
  // Allocate Client Characteristic Configuration table
  oadImgIdentifyConfig = (gattCharCfg_t *)osal_mem_alloc( sizeof(gattCharCfg_t) *
                                                          linkDBNumConns);
  if (oadImgIdentifyConfig == NULL)
  {
    return ( bleMemAllocError );
  }
  
  // Allocate Client Characteristic Configuration table
  oadImgBlockConfig = (gattCharCfg_t *)osal_mem_alloc( sizeof(gattCharCfg_t) *
                                                       linkDBNumConns);
  
  if (oadImgBlockConfig == NULL)
  {
    // Free already allocated data
    osal_mem_free( oadImgIdentifyConfig );
    
    return ( bleMemAllocError );
  }
  
  // Initialize Client Characteristic Configuration attributes
  GATTServApp_InitCharCfg( INVALID_CONNHANDLE, oadImgIdentifyConfig );
  GATTServApp_InitCharCfg( INVALID_CONNHANDLE, oadImgBlockConfig );

  return GATTServApp_RegisterService(oadAttrTbl, GATT_NUM_ATTRS(oadAttrTbl),
                                     GATT_MAX_ENCRYPT_KEY_SIZE, &oadCBs);
}

/**************************************************************************************************
 * @fn      HalLcdWriteString
 *
 * @brief   Write a string to the LCD
 *
 * @param   str    - pointer to the string that will be displayed
 *          option - display options
 *
 * @return  None
 **************************************************************************************************/
void HalLcdWriteString ( char *str, uint8 option)
{
#if (HAL_LCD == TRUE)
  uint8 strLen = 0;
  uint8 totalLen = 0;
  uint8 *buf;
  uint8 tmpLen;

  if ( Lcd_Line1 == NULL )
  {
    Lcd_Line1 = osal_mem_alloc( HAL_LCD_MAX_CHARS+1 );
    HalLcdWriteString( "TexasInstruments", 1 );
  }

  strLen = (uint8)osal_strlen( (char*)str );

  /* Check boundries */
  if ( strLen > HAL_LCD_MAX_CHARS )
    strLen = HAL_LCD_MAX_CHARS;

  if ( option == HAL_LCD_LINE_1 )
  {
    /* Line 1 gets saved for later */
    osal_memcpy( Lcd_Line1, str, strLen );
    Lcd_Line1[strLen] = '\0';
  }
  else
  {
    /* Line 2 triggers action */
    tmpLen = (uint8)osal_strlen( (char*)Lcd_Line1 );
    totalLen =  tmpLen + 1 + strLen + 1;
    buf = osal_mem_alloc( totalLen );
    if ( buf != NULL )
    {
      /* Concatenate strings */
      osal_memcpy( buf, Lcd_Line1, tmpLen );
      buf[tmpLen++] = ' ';
      osal_memcpy( &buf[tmpLen], str, strLen );
      buf[tmpLen+strLen] = '\0';

      /* Send it out */
#if defined (ZTOOL_P1) || defined (ZTOOL_P2)
      debug_str( (uint8*)buf );
#endif //ZTOOL_P1

      /* Free mem */
      osal_mem_free( buf );
    }
  }

  /* Display the string */
  HalLcd_HW_WriteLine (option, str);

#endif //HAL_LCD
}

/*********************************************************************
 * @fn      Glucose_AddService
 *
 * @brief   Initializes the Glucose   service by registering
 *          GATT attributes with the GATT server.
 *
 * @param   services - services to add. This is a bit map and can
 *                     contain more than one service.
 *
 * @return  Success or Failure
 */
bStatus_t Glucose_AddService(uint32 services)
{
    uint8 status;

    // Allocate Client Characteristic Configuration table
    glucoseMeasConfig = (gattCharCfg_t *)osal_mem_alloc( sizeof(gattCharCfg_t) *
                        linkDBNumConns );
    if ( glucoseMeasConfig == NULL )
    {
        return ( bleMemAllocError );
    }

    // Allocate Client Characteristic Configuration table
    glucoseContextConfig = (gattCharCfg_t *)osal_mem_alloc( sizeof(gattCharCfg_t) *
                           linkDBNumConns );
    if ( glucoseContextConfig == NULL )
    {
        // Free already allocated data
        osal_mem_free( glucoseMeasConfig );

        return ( bleMemAllocError );
    }

    // Allocate Client Characteristic Configuration table
    glucoseControlConfig = (gattCharCfg_t *)osal_mem_alloc( sizeof(gattCharCfg_t) *
                           linkDBNumConns );
    if ( glucoseControlConfig == NULL )
    {
        // Free already allocated data
        osal_mem_free( glucoseMeasConfig );
        osal_mem_free( glucoseContextConfig );

        return ( bleMemAllocError );
    }

    // Initialize Client Characteristic Configuration attributes.
    GATTServApp_InitCharCfg(INVALID_CONNHANDLE, glucoseMeasConfig);
    GATTServApp_InitCharCfg(INVALID_CONNHANDLE, glucoseContextConfig);
    GATTServApp_InitCharCfg(INVALID_CONNHANDLE, glucoseControlConfig);

    if (services & GLUCOSE_SERVICE)
    {
        // Register GATT attribute list and CBs with GATT Server App.
        status = GATTServApp_RegisterService(glucoseAttrTbl,
                                             GATT_NUM_ATTRS(glucoseAttrTbl),
                                             GATT_MAX_ENCRYPT_KEY_SIZE,
                                             &glucoseCBs);
    }
    else
    {
        status = SUCCESS;
    }

    return (status);
}

/*********************************************************************
 * @fn          ZDO_ParseSimpleDescBuf
 *
 * @brief       Parse a byte sequence representation of a Simple Descriptor.
 *
 * @param       buf  - pointer to a byte array representing a Simple Desc.
 * @param       desc - SimpleDescriptionFormat_t *
 *
 *              This routine allocates storage for the cluster IDs because
 *              they are 16-bit and need to be aligned to be properly processed.
 *              This routine returns non-zero if an allocation fails.
 *
 *              NOTE: This means that the caller or user of the input structure
 *                    is responsible for freeing the memory
 *
 * @return      0: success
 *              1: failure due to malloc failure.
 */
uint8 ZDO_ParseSimpleDescBuf( uint8 *buf, SimpleDescriptionFormat_t *desc )
{
  uint8 num, i;

  desc->EndPoint = *buf++;
  desc->AppProfId = BUILD_UINT16( buf[0], buf[1] );
  buf += 2;
  desc->AppDeviceId = BUILD_UINT16( buf[0], buf[1] );
  buf += 2;
  desc->AppDevVer = *buf >> 4;

  desc->Reserved = 0;
  buf++;

  // move in input cluster list (if any). allocate aligned memory.
  num = desc->AppNumInClusters = *buf++;
  if ( num )
  {
    if (!(desc->pAppInClusterList = (uint16 *)osal_mem_alloc(num*sizeof(uint16))))
    {
      // malloc failed. we're done.
      return 1;
    }
    for (i=0; i<num; ++i)
    {
      desc->pAppInClusterList[i] = BUILD_UINT16( buf[0], buf[1] );
      buf += 2;
    }
  }

  // move in output cluster list (if any). allocate aligned memory.
  num = desc->AppNumOutClusters = *buf++;
  if (num)
  {
    if (!(desc->pAppOutClusterList = (uint16 *)osal_mem_alloc(num*sizeof(uint16))))
    {
      // malloc failed. free input cluster list memory if there is any
      if ( desc->pAppInClusterList != NULL )
      {
        osal_mem_free(desc->pAppInClusterList);
      }
      return 1;
    }
    for (i=0; i<num; ++i)
    {
      desc->pAppOutClusterList[i] = BUILD_UINT16( buf[0], buf[1] );
      buf += 2;
    }
  }
  return 0;
}

/*********************************************************************
 * @fn      afRegisterExtended
 *
 * @brief   Register an Application's EndPoint description.
 *
 * @param   epDesc - pointer to the Application's endpoint descriptor.
 * @param   descFn - pointer to descriptor callback function
 *
 * NOTE:  The memory that epDesc is pointing to must exist after this call.
 *
 * @return  Pointer to epList_t on success, NULL otherwise.
 */
epList_t *afRegisterExtended( endPointDesc_t *epDesc, pDescCB descFn )
{
  epList_t *ep;
  epList_t *epSearch;

  ep = osal_mem_alloc( sizeof ( epList_t ) );
  if ( ep )
  {
    // Fill in the new list entry
    ep->epDesc = epDesc;

    // Default to allow Match Descriptor.
    ep->flags = eEP_AllowMatch;
    ep->pfnDescCB = descFn;
    ep->nextDesc = NULL;

    // Does a list exist?
    if ( epList == NULL )
      epList = ep;  // Make this the first entry
    else
    {
      // Look for the end of the list
      epSearch = epList;
      while( epSearch->nextDesc != NULL )
        epSearch = epSearch->nextDesc;

      // Add new entry to end of list
      epSearch->nextDesc = ep;
    }
  }

  return ep;
}

/*********************************************************************
 * @fn      osalInitTasks
 *
 * @brief   This function invokes the initialization function for each task.
 *
 * @param   void
 *
 * @return  none
 */
void osalInitTasks( void )
{
  uint8 taskID = 0;

  tasksEvents = (uint16 *)osal_mem_alloc( sizeof( uint16 ) * tasksCnt);
  osal_memset( tasksEvents, 0, (sizeof( uint16 ) * tasksCnt));

  macTaskInit( taskID++ );
  nwk_init( taskID++ );
  Hal_Init( taskID++ );
#if defined( MT_TASK )
  MT_TaskInit( taskID++ );
#endif
  APS_Init( taskID++ );
#if defined ( ZIGBEE_FRAGMENTATION )
  APSF_Init( taskID++ );
#endif
#if defined ( INTER_PAN)
  StubAPS_Init( taskID++ );
#endif
  ZDApp_Init( taskID++ );
#if defined ( ZIGBEE_FREQ_AGILITY ) || defined ( ZIGBEE_PANID_CONFLICT )
  ZDNwkMgr_Init( taskID++ );
#endif
  BaseED_Init( taskID++ );
  EndDeviceTask_Init( taskID++ );
#if defined ( MSP_REPROGRAM )
  MSPTask_Init( taskID++ );
#endif
}

/*********************************************************************
 * @fn      osal_mem_init
 *
 * @brief   Initialize the heap memory management system.
 *
 * @param   void
 *
 * @return  void
 */
void osal_mem_init( void )
{
  osalMemHdr_t *tmp;

#if ( OSALMEM_PROFILER )
  (void)osal_memset( theHeap, OSALMEM_INIT, MAXMEMHEAP );
#endif

  // Setup a NULL block at the end of the heap for fast comparisons with zero.
  tmp = (osalMemHdr_t *)theHeap + (MAXMEMHEAP / HDRSZ) - 1;
  *tmp = 0;

  // Setup a small-block bucket.
  tmp = (osalMemHdr_t *)theHeap;
  *tmp = SMALLBLKHEAP;

  // Setup the wilderness.
  tmp = (osalMemHdr_t *)theHeap + (SMALLBLKHEAP / HDRSZ);
  *tmp = ((MAXMEMHEAP / HDRSZ) * HDRSZ) - SMALLBLKHEAP - HDRSZ;

  // Setup a NULL block that is never freed so that the small-block bucket
  // is never coalesced with the wilderness.
  ff1 = tmp;
  ff2 = osal_mem_alloc( 0 );
  ff1 = (osalMemHdr_t *)theHeap;

#if ( OSALMEM_METRICS )
  /* Start with the small-block bucket and the wilderness - don't count the
   * end-of-heap NULL block nor the end-of-small-block NULL block.
   */
  blkCnt = blkFree = 2;
#endif
}

/*********************************************************************
 * @fn      osalInitTasks
 *
 * @brief   This function invokes the initialization function for each task.
 *
 * @param   void
 *
 * @return  none
 */
void osalInitTasks( void )
{
  uint8 taskID = 0;

  tasksEvents = (uint16 *)osal_mem_alloc( sizeof( uint16 ) * tasksCnt);
  osal_memset( tasksEvents, 0, (sizeof( uint16 ) * tasksCnt));

  macTaskInit( taskID++ );
  nwk_init( taskID++ );
  Hal_Init( taskID++ );
#if defined( MT_TASK )
  MT_TaskInit( taskID++ );
#endif
  APS_Init( taskID++ );
#if defined ( ZIGBEE_FRAGMENTATION )
  APSF_Init( taskID++ );
#endif
  ZDApp_Init( taskID++ );
#if defined ( ZIGBEE_FREQ_AGILITY ) || defined ( ZIGBEE_PANID_CONFLICT )
  ZDNwkMgr_Init( taskID++ );
#endif
#if defined( INTER_PAN )
  StubAPS_Init( taskID++ );
#endif
  zcl_Init( taskID++ );
#if defined ( ZCL_KEY_ESTABLISH )
  zclGeneral_KeyEstablish_Init( taskID++ );
#endif
  ipd_Init( taskID );
}

/*******************************************************************************
 * @fn      zclPI_Send_11073ConnectReq
 *
 * @brief   Call to send out an 11073 Connect Request Command. This command
 *          is generated when a Data Management device wishes to connect to
 *          an 11073 agent device. This may be in response to receiving a 
 *          connect status notification command from that agent device with
 *          the connect status field set to RECONNECT_REQUEST.
 *
 * @param   srcEP - Sending application's endpoint
 * @param   dstAddr - where you want the message to go
 * @param   connectCtrl - connect control
 * @param   idleTimeout - inactivity time (in minutes) which Data Management device
 *                        will wait w/o receiving any data before it disconnects
 * @param   managerAddr - IEEE address (64-bit) of Data Management device 
 *                        transmitting this frame
 * @param   managerEP - source endpoint from which Data Management device is
                        transmitting this frame
 * @param   disableDefaultRsp - whether to disable the Default Response command
 * @param   seqNum - sequence number
 *
 * @return  ZStatus_t
 */
ZStatus_t zclPI_Send_11073ConnectReq( uint8 srcEP, afAddrType_t *dstAddr,
                                      uint8 connectCtrl, uint16 idleTimeout,
                                      uint8 *managerAddr, uint8 managerEP, 
                                      uint8 disableDefaultRsp, uint8 seqNum )
{
  uint8 *buf;
  uint8 msgLen = 1 + 2 + Z_EXTADDR_LEN + 1; // connect ctrl + idle timeout + IEEE Address + manager EP
  ZStatus_t stat;

  buf = osal_mem_alloc( msgLen );
  if ( buf )
  {
    buf[0] = connectCtrl;
    buf[1] = LO_UINT16( idleTimeout );
    buf[2] = HI_UINT16( idleTimeout );
    osal_memcpy( &(buf[3]), managerAddr, Z_EXTADDR_LEN );
    buf[11] = managerEP;

    stat = zcl_SendCommand( srcEP, dstAddr, ZCL_CLUSTER_ID_PI_11073_PROTOCOL_TUNNEL,
                            COMMAND_PI_11073_TUNNEL_CONNECT_REQ, TRUE, 
                            ZCL_FRAME_CLIENT_SERVER_DIR, disableDefaultRsp, 0, seqNum,
                            msgLen, buf );
    osal_mem_free( buf );
  }
  else
  {
    stat = ZMemError;
  }
  
  return ( stat );
}

/*********************************************************************
 * @fn      osalInitTasks
 *
 * @brief   This function invokes the initialization function for each task.
 *
 * @param   void
 *
 * @return  none
 */
void osalInitTasks( void )
{
  uint8 taskID = 0;

  tasksEvents = (uint16 *)osal_mem_alloc( sizeof( uint16 ) * tasksCnt);
  osal_memset( tasksEvents, 0, (sizeof( uint16 ) * tasksCnt));

  /* LL Task */
  LL_Init( taskID++ );

  /* Hal Task */
  Hal_Init( taskID++ );

  /* HCI Task */
  HCI_Init( taskID++ );

#if defined ( OSAL_CBTIMER_NUM_TASKS )
  /* Callback Timer Tasks */
  osal_CbTimerInit( taskID );
  taskID += OSAL_CBTIMER_NUM_TASKS;
#endif

  /* GAP Task */
  GAP_Init( taskID++ );

  /* Profiles */
  GAPRole_Init( taskID++ );

  /* Application */
  SimpleBLEBroadcaster_Init( taskID );
}

/*********************************************************************
 * @fn      osalInitTasks
 *
 * @brief   This function invokes the initialization function for each task.
 *
 * @param   void
 *
 * @return  none
 */
void osalInitTasks( void )
{
  uint8 taskID = 0;

  tasksEvents = (uint16 *)osal_mem_alloc( sizeof( uint16 ) * tasksCnt);
  osal_memset( tasksEvents, 0, (sizeof( uint16 ) * tasksCnt));

  macTaskInit( taskID++ );
  nwk_init( taskID++ );
  Hal_Init( taskID++ );
#if defined( MT_TASK )
  MT_TaskInit( taskID++ );
#endif
  APS_Init( taskID++ );
#if defined ( ZIGBEE_FRAGMENTATION )
  APSF_Init( taskID++ );
#endif
  ZDApp_Init( taskID++ );
#if defined ( ZIGBEE_FREQ_AGILITY ) || defined ( ZIGBEE_PANID_CONFLICT )
  ZDNwkMgr_Init( taskID++ );
#endif
  zcl_Init( taskID++ );
  zclSampleSw_Init( taskID++ );
#if (defined OTA_CLIENT) && (OTA_CLIENT == TRUE)
  zclOTA_Init( taskID );
#endif
}

 /***************************************************************************************************
 * @fn      MT_ZllNotifyTL
 *
 * @brief   Process and send Indication for a successful ZLL Touch-Link over MT_APP.
 *
 * @param   pRec - Target's information record
 *
 * @return  none
 ***************************************************************************************************/
void MT_ZllNotifyTL( epInfoRec_t *pRec )
{
  byte *pBuf;
  pBuf = osal_mem_alloc( ZLL_CMDLENOPTIONAL_GET_EP_LIST_RSP ); // endpoint information record entry
  if ( pBuf )
  {
    pBuf[0] = LO_UINT16( pRec->nwkAddr );
    pBuf[1] = HI_UINT16( pRec->nwkAddr );

    pBuf[2] = pRec->endpoint;

    pBuf[3] = LO_UINT16( pRec->profileID );
    pBuf[4] = HI_UINT16( pRec->profileID );

    pBuf[5] = LO_UINT16( pRec->deviceID );
    pBuf[6] = HI_UINT16( pRec->deviceID );

    pBuf[7] = pRec->version;

    /* Send out Reset Response message */
    MT_BuildAndSendZToolResponse( ((uint8)MT_RPC_CMD_AREQ | (uint8)MT_RPC_SYS_APP),
                                   MT_APP_ZLL_TL_IND,
                                   ZLL_CMDLENOPTIONAL_GET_EP_LIST_RSP, pBuf );

    osal_mem_free( pBuf );
  }
}

/*********************************************************************
 * @fn      SimpleProfile_AddService
 *
 * @brief   Initializes the Simple Profile service by registering
 *          GATT attributes with the GATT server.
 *
 * @param   services - services to add. This is a bit map and can
 *                     contain more than one service.
 *
 * @return  Success or Failure
 */
bStatus_t SimpleProfile_AddService( uint32 services )
{
  uint8 status = SUCCESS;

  // Allocate Client Characteristic Configuration table
  simpleProfileChar4Config = (gattCharCfg_t *)osal_mem_alloc( sizeof(gattCharCfg_t) *
                                                              linkDBNumConns );
  if ( simpleProfileChar4Config == NULL )
  {
    return ( bleMemAllocError );
  }

  // Initialize Client Characteristic Configuration attributes
  GATTServApp_InitCharCfg( INVALID_CONNHANDLE, simpleProfileChar4Config );

  // Register with Link DB to receive link status change callback
  VOID linkDB_Register( simpleProfile_HandleConnStatusCB );

  if ( services & SIMPLEPROFILE_SERVICE )
  {
    // Register GATT attribute list and CBs with GATT Server App
    status = GATTServApp_RegisterService( simpleProfileAttrTbl,
                                          GATT_NUM_ATTRS( simpleProfileAttrTbl ),
                                          GATT_MIN_ENCRYPT_KEY_SIZE,
                                          &simpleProfileCBs );
  }

  return ( status );
}

/***************************************************************************************************
 * @fn      MT_AppPB_ZCLInd
 *
 * @brief   Send an MT_APP_PB_ZCL_IND command
 *
 * @param   pInd - pointer to the indication
 *
 * @return  void
 ***************************************************************************************************/
void MT_AppPB_ZCLInd( mtAppPB_ZCLInd_t *pInd )
{
  uint8 *pData;
  uint8 *pBuf;
  uint8 len;

  len = MT_APP_PB_ZCL_IND_HDR_LEN + pInd->appPBDataLen;

  pData = (uint8 *)osal_mem_alloc( len );
  if ( pData != NULL )
  {
    pBuf = pData;
    *pBuf++ = pInd->appEP;
    *pBuf++ = LO_UINT16( pInd->srcAddr );
    *pBuf++ = HI_UINT16( pInd->srcAddr );
    *pBuf++ = pInd->srcEP;
    *pBuf++ = LO_UINT16( pInd->clusterID );
    *pBuf++ = HI_UINT16( pInd->clusterID );
    *pBuf++ = pInd->commandID;
    *pBuf++ = pInd->specific;
    *pBuf++ = pInd->direction;
    *pBuf++ = pInd->disableDefRsp;
    *pBuf++ = LO_UINT16( pInd->manuCode );
    *pBuf++ = HI_UINT16( pInd->manuCode );
    *pBuf++ = pInd->transSeqNum;
    osal_memcpy( pBuf, pInd->appPBData, pInd->appPBDataLen );

    MT_BuildAndSendZToolResponse( ( (uint8)MT_RPC_CMD_AREQ | (uint8)MT_RPC_SYS_APP ),
                                  MT_APP_PB_ZCL_IND, len, pData );
    osal_mem_free( pData );
  }
}

/*******************************************************************************
 * @fn      zclPI_Send_AdvertiseProtocolAddrCmd
 *
 * @brief   Call to send out an Advertise Protocol Address Command. This command
 *          is sent out typically upon startup or whenever the Protocol Address
 *          attribute changes. It is typically multicast to a group of inter-
 *          communicating Generic Tunnel clusters.
 *
 * @param   srcEP - Sending application's endpoint
 * @param   dstAddr - where you want the message to go
 * @param   len - length of protocol address
 * @param   protocolAddr - protocol address
 * @param   disableDefaultRsp - whether to disable the Default Response command
 * @param   seqNum - sequence number
 *
 * @return  ZStatus_t
 */
ZStatus_t zclPI_Send_AdvertiseProtocolAddrCmd( uint8 srcEP, afAddrType_t *dstAddr,
                                               uint8 len, uint8 *protocolAddr, 
                                               uint8 disableDefaultRsp, uint8 seqNum )
{
  uint8 *buf;
  ZStatus_t stat;

  buf = osal_mem_alloc( len+1 ); // 1 for length field
  if ( buf )
  {  
    buf[0] = len;
    osal_memcpy( &(buf[1]), protocolAddr, len );

    stat = zcl_SendCommand( srcEP, dstAddr, ZCL_CLUSTER_ID_PI_GENERIC_TUNNEL,
                            COMMAND_PI_GENERIC_TUNNEL_ADVERTISE_PROTOCOL_ADDR, TRUE, 
                            ZCL_FRAME_SERVER_CLIENT_DIR, disableDefaultRsp, 0, seqNum,
                            (len+1), buf );
    osal_mem_free( buf );
  }
  else
  {
    stat = ZMemError;
  }
  
  return ( stat );
}

/*******************************************************************************
 * @fn      zclPI_Send_11073TransferAPDUCmd
 *
 * @brief   Call to send out an 11073 Transfer APDU Command. This command is 
 *          used when an 11073 network layer wishes to transfer an 11073 APDU 
 *          across a ZigBee tunnel to another 11073 network layer.
 *
 *          The most stringent reliability characteristic of a given transport
 *          technology is “Best” reliability. Note - For ZigBee, this corresponds
 *          to use of APS-ACKs.
 *
 *          The least stringent reliability characteristic of a given transport
 *          technology is “Good” reliability. Note - For ZigBee, this corresponds
 *          to no use of APS-ACKs.
 *
 *          Note: This command shall always be transmitted with the Disable Default 
 *          Response bit in the ZCL frame control field set to 1.
 *
 * @param   srcEP - Sending application's endpoint
 * @param   dstAddr - where you want the message to go
 * @param   len - length of APDU
 * @param   apdu - APDU to be sent
 * @param   seqNum - sequence number
 *
 * @return  ZStatus_t
 */
ZStatus_t zclPI_Send_11073TransferAPDUCmd( uint8 srcEP, afAddrType_t *dstAddr,
                                           uint16 len, uint8 *apdu, uint8 seqNum )
{
  uint8 *buf;
  ZStatus_t stat;

  buf = osal_mem_alloc( len+2 ); // 2 for length field (long octet string)
  if ( buf )
  {  
    buf[0] = LO_UINT16( len );
    buf[1] = HI_UINT16( len );
    osal_memcpy( &(buf[2]), apdu, len );

    // This command shall always be transmitted with the Disable Default 
    // Response bit in the ZCL frame control field set to 1.
    stat = zcl_SendCommand( srcEP, dstAddr, ZCL_CLUSTER_ID_PI_11073_PROTOCOL_TUNNEL,
                            COMMAND_PI_11073_TUNNEL_TRANSFER_APDU, TRUE, 
                            ZCL_FRAME_CLIENT_SERVER_DIR, TRUE, 0, seqNum, (len+2), buf );
    osal_mem_free( buf );
  }
  else
  {
    stat = ZMemError;
  }
  
  return ( stat );
}

/* @fn      osalInitTasks
 * @brief   This function invokes the initialization function for each task.
 * @param   void
 * @return  none*/
void osalInitTasks( void ) {
  uint8 taskID = 0;
  tasksEvents = (uint16 *)osal_mem_alloc( sizeof( uint16 ) * tasksCnt);
  osal_memset( tasksEvents, 0, (sizeof( uint16 ) * tasksCnt));
#if defined( ZMAC_F8W )
  macTaskInit( taskID++ );
#endif
#if !defined( NONWK )
  nwk_init( taskID++ );
#endif
  Hal_Init( taskID++ );
#if defined( MT_TASK )
  MT_TaskInit( taskID++ );
#endif
#if !defined( NONWK )
  APS_Init( taskID++ );
  ZDApp_Init( taskID++ );
#endif
  LoacationApp_Init( taskID++ );
#if defined ( LOCATION_REFNODE )
  RefNode_Init( taskID );
#endif
#if defined ( LOCATION_BLINDNODE )
  BlindNode_Init( taskID );
#endif
#if defined ( LOCATION_GATEWAY )
  Gateway_Init( taskID );
#endif
}

int assignHuffmanCode(struct MinHeapNode* root, uint8 *c_ptr, uint16 c_idx)
{
    if (root->left)
    {
        fillHuffmanCode(c_ptr, c_idx, 0);
        if (assignHuffmanCode(root->left, c_ptr, c_idx + 1) == 1) {
            return 1;
        }
    }

    if (root->right)
    {
        fillHuffmanCode(c_ptr, c_idx, 1);
        if (assignHuffmanCode(root->right, c_ptr, c_idx + 1) == 1) {
            return 1;
        }
    }

    // If this is a leaf node, then it contains one of the final code 
    if (isLeaf(root)) {
        hmap[root->data].bits = c_idx;
        hmap[root->data].b_ptr = osal_mem_alloc((c_idx + 7) / 8);
        if (hmap[root->data].b_ptr == NULL) {
            hError |= (1 << 7);
            return 1;
        }
        osal_memcpy(hmap[root->data].b_ptr, c_ptr, ((c_idx + 7) / 8));
    }
    return 0;
}

/***************************************************************************************************
 * @fn          nwk_MTCallbackSubDataIndication
 *
 * @brief       Process the callback subscription for NLDE-DATA.indication
 *
 * @param       SrcAddress      - 16 bit address
 * @param       nsduLength      - Length of incoming data
 * @param       nsdu            - Pointer to incoming data
 * @param       LinkQuality     - Link quality measured during
 *                                reception.
 *
 * @return      none
 ***************************************************************************************************/
void nwk_MTCallbackSubDataIndication(uint16 SrcAddress, int16 nsduLength, uint8 *nsdu, uint8 LinkQuality)
{
  uint8 *msgPtr;
  uint8 *msg;
  uint8 msgLen;

  msgLen = sizeof( uint16 ) + sizeof( uint8 ) + ZTEST_DEFAULT_DATA_LEN
            + sizeof( uint8);

  msgPtr = osal_mem_alloc( msgLen );
  if ( msgPtr )
  {
    //Fill up the data bytes
    msg = msgPtr;

    //First fill in details
    *msg++ = LO_UINT16( SrcAddress );
    *msg++ = HI_UINT16( SrcAddress );

    //Since the max packet size is less than 255 bytes, a byte is enough
    //to represent nsdu length
    *msg++ = ( uint8 ) nsduLength;

    osal_memset( msg, NULL, ZTEST_DEFAULT_DATA_LEN ); // Clear the mem
    osal_memcpy( msg, nsdu, nsduLength );
    msg += ZTEST_DEFAULT_DATA_LEN;

    *msg++ = LinkQuality;

    MT_BuildAndSendZToolResponse( ((uint8)MT_RPC_CMD_AREQ | (uint8)MT_RPC_SYS_NWK), MT_NLDE_DATA_IND, msgLen, msgPtr );

    osal_mem_free( msgPtr );
  }
}