C++ osal_memcpy函数代码示例

/* wcn_compressor_init - create a compressor and do init
 * @ name - compressor's name
 * @ L1_buf_sz - L1 buffer size
 * @ L2_buf_sz - L2 buffer size
 *
 * Retunr object's pointer if success, else NULL
 */
P_WCN_COMPRESSOR_T wcn_compressor_init(PUINT8 name, INT32 L1_buf_sz, INT32 L2_buf_sz)
{
    z_stream *pstream = NULL;
    P_WCN_COMPRESSOR_T compress = NULL;
    
    compress = (P_WCN_COMPRESSOR_T)osal_malloc(sizeof(WCN_COMPRESSOR_T));
    if (!compress) {
        STP_DBG_ERR_FUNC("alloc compressor failed!\n");
        goto fail;
    }
    
    osal_memset(compress, 0, sizeof(WCN_COMPRESSOR_T));
    osal_memcpy(compress->name, name, STP_OJB_NAME_SZ);

    compress->f_compress_en = 0;
    compress->compress_type = GZIP;
    
    if (compress->compress_type == GZIP) {
        compress->worker = osal_malloc(sizeof(z_stream));
        if (!compress->worker) {
            STP_DBG_ERR_FUNC("alloc stream failed!\n");
            goto fail;
        }
        pstream = (z_stream*)compress->worker;
            
        pstream->workspace = osal_malloc(zlib_deflate_workspacesize(MAX_WBITS, MAX_MEM_LEVEL));
        if (!pstream->workspace) {
            STP_DBG_ERR_FUNC("alloc workspace failed!\n");
            goto fail;
        }
	zlib_deflateInit2(pstream, Z_DEFAULT_COMPRESSION, Z_DEFLATED, -MAX_WBITS,
			  DEF_MEM_LEVEL, Z_DEFAULT_STRATEGY);
    }
    
    compress->handler = wcn_gzip_compressor;
    compress->L1_buf_sz = L1_buf_sz;
    compress->L2_buf_sz = L2_buf_sz;
    compress->L1_pos = 0;
    compress->L2_pos = 0;
    compress->uncomp_size = 0;
    compress->crc32 = 0xffffffffUL;

    compress->L1_buf = osal_malloc(compress->L1_buf_sz);
    if (!compress->L1_buf) {
        STP_DBG_ERR_FUNC("alloc %d bytes for L1 buf failed!\n", compress->L1_buf_sz);
        goto fail;
    }

    compress->L2_buf = osal_malloc(compress->L2_buf_sz);
    if (!compress->L2_buf) {
        STP_DBG_ERR_FUNC("alloc %d bytes for L2 buf failed!\n", compress->L2_buf_sz);
        goto fail;
    }

    STP_DBG_INFO_FUNC("create compressor OK! L1 %d bytes, L2 %d bytes\n", L1_buf_sz, L2_buf_sz);
    return compress;

fail:
    if (compress) {
        if (compress->L2_buf) {
            osal_free(compress->L2_buf);
            compress->L2_buf = NULL;
        }

        if (compress->L1_buf) {
            osal_free(compress->L1_buf);
            compress->L1_buf = NULL;
        }

        if (compress->worker) {
            pstream = (z_stream*)compress->worker;
            if ((compress->compress_type == GZIP) && pstream->workspace) {
                zlib_deflateEnd(pstream);
                osal_free(pstream->workspace);
            }
            osal_free(compress->worker);
            compress->worker = NULL;
        }
            
        if (compress->worker) {
            osal_free(compress->worker);
            compress->worker = NULL;
        }
        
        osal_free(compress);
        compress = NULL;
    }

    STP_DBG_ERR_FUNC("init failed!\n");
     
    return NULL;
}

/*********************************************************************
 * @fn      SimpleProfile_GetParameter
 *
 * @brief   Get a Simple Profile parameter.
 *
 * @param   param - Profile parameter ID
 * @param   value - pointer to data to put.  This is dependent on
 *          the parameter ID and WILL be cast to the appropriate 
 *          data type (example: data type of uint16 will be cast to 
 *          uint16 pointer).
 *
 * @return  bStatus_t
 */
bStatus_t SimpleProfile_GetParameter( uint8 param, void *value, uint8 *returnBytes)
{
  bStatus_t ret = SUCCESS;
  switch ( param )
  {
    case SIMPLEPROFILE_CHAR1:
      *((uint8*)value) = simpleProfileChar1;
      *returnBytes = 1;
      break;

    case SIMPLEPROFILE_CHAR2:
      *((uint8*)value) = simpleProfileChar2;
      *returnBytes = 1;
      break;      

    case SIMPLEPROFILE_CHAR3:
      *((uint8*)value) = simpleProfileChar3;
      *returnBytes = 1;
      break;  

    case SIMPLEPROFILE_CHAR4:
      *((uint8*)value) = simpleProfileChar4;
      *returnBytes = 1;
      break;

    case SIMPLEPROFILE_CHAR5:
      VOID osal_memcpy( value, simpleProfileChar5, simpleProfileChar5Len );
      *returnBytes = simpleProfileChar5Len;
      break;      
      
    case SIMPLEPROFILE_CHAR6:
      VOID osal_memcpy( value, simpleProfileChar6, simpleProfileChar6Len );
      *returnBytes = simpleProfileChar6Len;
      break;  
      
    case SIMPLEPROFILE_CHAR7:
      VOID osal_memcpy( value, simpleProfileChar7, simpleProfileChar7Len );
      *returnBytes = simpleProfileChar7Len;
      break;  
      
    case SIMPLEPROFILE_CHAR8:
      VOID osal_memcpy( value, simpleProfileChar8, simpleProfileChar8Len );
      *returnBytes = simpleProfileChar8Len;
      break;  

    case SIMPLEPROFILE_CHAR9:
      VOID osal_memcpy( value, simpleProfileChar9, simpleProfileChar9Len );
      *returnBytes = simpleProfileChar9Len;
      break;  

    case SIMPLEPROFILE_CHARA:
      VOID osal_memcpy( value, simpleProfileCharA, simpleProfileCharALen );
      *returnBytes = simpleProfileCharALen;
      break;  

    default:
      ret = INVALIDPARAMETER;
      *returnBytes = 0;
      break;
  }
  
  return ( ret );
}

/*********************************************************************
 * @fn      simpleProfile_WriteAttrCB
 *
 * @brief   Validate attribute data prior to a write operation
 *
 * @param   connHandle - connection message was received on
 * @param   pAttr - pointer to attribute
 * @param   pValue - pointer to data to be written
 * @param   len - length of data
 * @param   offset - offset of the first octet to be written
 *
 * @return  Success or Failure
 */
static bStatus_t simpleProfile_WriteAttrCB( uint16 connHandle, gattAttribute_t *pAttr,
                                 uint8 *pValue, uint8 len, uint16 offset )
{
  bStatus_t status = SUCCESS;
  uint8 notifyApp = 0xFF;
  
  // If attribute permissions require authorization to write, return error
  if ( gattPermitAuthorWrite( pAttr->permissions ) )
  {
    // Insufficient authorization
    return ( ATT_ERR_INSUFFICIENT_AUTHOR );
  }
  
  if ( pAttr->type.len == ATT_BT_UUID_SIZE )
  {
    // 16-bit UUID
    uint16 uuid = BUILD_UINT16( pAttr->type.uuid[0], pAttr->type.uuid[1]);
    switch ( uuid )
    {
      case SIMPLEPROFILE_CHAR1_UUID:
      case SIMPLEPROFILE_CHAR3_UUID:

        //Validate the value
        // Make sure it's not a blob oper
        if ( offset == 0 )
        {
          if ( len != 1 )
          {
            status = ATT_ERR_INVALID_VALUE_SIZE;
          }
        }
        else
        {
          status = ATT_ERR_ATTR_NOT_LONG;
        }
        
        //Write the value
        if ( status == SUCCESS )
        {
          uint8 *pCurValue = (uint8 *)pAttr->pValue;        
          *pCurValue = pValue[0];

          if( pAttr->pValue == &simpleProfileChar1 )
          {
            notifyApp = SIMPLEPROFILE_CHAR1;        
          }
          else
          {
            notifyApp = SIMPLEPROFILE_CHAR3;           
          }
        }
             
        break;

      case SIMPLEPROFILE_CHAR5_UUID:
        if ( offset == 0 )
        {
          //if ( len != SIMPLEPROFILE_CHAR6_LEN )
          if ( len > SIMPLEPROFILE_CHAR5_LEN )
          {
            status = ATT_ERR_INVALID_VALUE_SIZE;
          }
        }
        else
        {
          status = ATT_ERR_ATTR_NOT_LONG;
        }

        //Write the value
        if ( status == SUCCESS )
        {
	        VOID osal_memcpy( pAttr->pValue, pValue, len );
            simpleProfileChar5Len = len;
            notifyApp = SIMPLEPROFILE_CHAR5;
        }
             
        break;        
        
      case SIMPLEPROFILE_CHAR6_UUID:
        //Validate the value
        // Make sure it's not a blob oper
        //LCD_WRITE_STRING_VALUE( "WriteAttrCB 6 len=", len, 10, HAL_LCD_LINE_1 );
        //LCD_WRITE_STRING_VALUE( "WriteAttrCB 6 len2=", simpleProfileChar6Len, 10, HAL_LCD_LINE_1 );
        if ( offset == 0 )
        {
          //if ( len != SIMPLEPROFILE_CHAR6_LEN )
          if ( len > SIMPLEPROFILE_CHAR6_LEN )
//.........这里部分代码省略.........

/**************************************************************************************************
 * @fn          pulseTestingDataRx
 *
 * @brief       This function is called by pulseAfMsgRx() to process incoming data  This is not used in 
 *              normal operation.  This is used for testing system with differnt payload sizes.
 * input parameters
 *
 * @param       msg - A pointer to the afIncomingMSGPacket_t packet.
 *
 * output parameters
 *
 * None.
 *
 * @return      None.
 **************************************************************************************************
 */
static void pulseTestingDataRx(afIncomingMSGPacket_t *msg)
{
   uint8 fcs = 0, idx;
   int8 Rssi;
   //Convert LQI to RSSI
   Rssi =(int8)(((msg->LinkQuality * 97)/0xFF) -87); 
   //uint8 *TestRxBuffer;
   //TestRxBuffer = (uint8*)osal_mem_alloc(MHMS_TEST_BUFF_LEN);
    
  

  // Last announce broadcast to stop must have expired before a parent could forward to a ZED.
  if (INVALID_NODE_ADDR == pulseAddr)
  {
    (void)osal_set_event(pulseTaskId, PULSE_EVT_ANN);
  }

  TestRxBuffer[PULSE_SOP_IDX] = PULSE_SOP_VAL;
  TestRxBuffer[PULSE_ADR_LSB] = LO_UINT16(msg->srcAddr.addr.shortAddr);
  TestRxBuffer[PULSE_ADR_MSB] = HI_UINT16(msg->srcAddr.addr.shortAddr);

  // 1st byte of message is skipped - CMD is always 0 for data.
  (void)osal_memcpy(TestRxBuffer+PULSE_DAT_OFF, msg->cmd.Data+1, MHMS_TEST_PAYLOAD_LEN-1);  //MHMS copies one buffer to another

  for (idx = PULSE_ADR_LSB; idx < MHMS_FCS_IDX; idx++)
  {
    fcs ^= TestRxBuffer[idx];
  }
  TestRxBuffer[idx] = fcs;
  
  uint8 PktSeqNum; 
  
  uint8 parentAddrLSB;
  uint8 parentAddrMSB;
  uint8 tsensorBuf[15];
  
  parentAddrLSB= TestRxBuffer[11];
  parentAddrMSB= TestRxBuffer[12];  
  
  PktSeqNum = TestRxBuffer[13];
  
  //Start of Frame Delimiter
  tsensorBuf[0]=0xFE;
  tsensorBuf[1]=10;
  tsensorBuf[2]=LO_UINT16(0x8746);
  tsensorBuf[3]=HI_UINT16(0x8746);
  
  //Source Address
  tsensorBuf[4] = LO_UINT16(msg->srcAddr.addr.shortAddr);
  tsensorBuf[5] = HI_UINT16(msg->srcAddr.addr.shortAddr);
  
  tsensorBuf[6]=LO_UINT16(2);  //MHMS are 6 - 9 being used for anything?
  tsensorBuf[7]=HI_UINT16(2);
  tsensorBuf[8]=LO_UINT16(4);
  tsensorBuf[9]=HI_UINT16(4);
  
  //Temperature and Voltage Data
  tsensorBuf[10]= Rssi;
  tsensorBuf[11]= PktSeqNum; 
  
  //Parent Address
  tsensorBuf[12]= parentAddrLSB;
  tsensorBuf[13]= parentAddrMSB;


  //FCS Check on the middle 13 bytes
  tsensorBuf[14] = calcFCS(&tsensorBuf[1], 13 );

  HalUARTWrite(PULSE_PORT, tsensorBuf, 15);  //For communicating with the Zigbee sensor Monitor
  
  //osal_mem_free(TestRxBuffer);
}

/********************************************************************************************************
 * @fn      ZMacSecuritySetReq
 *
 * @brief   Write a MAC Security PIB attribute.
 *
 * @param   attr - PIB attribute to Set
 * @param   value - pointer to the data
 *
 * @return  status
 ********************************************************************************************************/
uint8 ZMacSecuritySetReq( uint8 attr, byte *value )
{
  uint8         keyIndex, entry;
  uint32        frameCounter;
  uint8        *ptr;
  uint8         tmp[MAX_SECURITY_PIB_SET_ENTRY]; /* This must cover the largest single PIB entry */
  uint8         temp[MAX_SECURITY_PIB_SET_ENTRY]; /* This must cover the largest single PIB entry */

  /* Note that the Transform byte array to propriety PIB struct below works 
   * because the members of the structures are all defined manually in a way, 
   * where memcpy would work, without usage of explicit 'pack', they fit 
   * such that memcpy would work
   */
  ptr = (uint8 *) value;
  switch (attr)
  {
    case ZMacKeyIdLookupEntry:
      keyIndex = *ptr++; /* key_index */
      entry    = *ptr++; /* key_id_lookup_index */
      
      /* Transform byte array to propriety PIB struct */
      osal_memcpy(tmp, ptr, sizeof(keyIdLookupDescriptor_t));
      osal_memcpy(&((macSecurityPibKeyIdLookupEntry_t *)temp)->macKeyIdLookupEntry, tmp, sizeof(keyIdLookupDescriptor_t));
      ((macSecurityPibKeyIdLookupEntry_t *)temp)->key_id_lookup_index = entry;
      ((macSecurityPibKeyIdLookupEntry_t *)temp)->key_index = keyIndex;
      return (ZMacStatus_t) MAC_MlmeSetSecurityReq( attr, temp );
      break;
      
    case ZMacKeyDeviceEntry:
      keyIndex = *ptr++; /* key_index */
      entry    = *ptr++; /* key_device_index */
      
      /* Transform byte array to propriety PIB struct */
      osal_memcpy(tmp, ptr, sizeof(keyDeviceDescriptor_t));
      osal_memcpy(&((macSecurityPibKeyDeviceEntry_t *)temp)->macKeyDeviceEntry, tmp, sizeof(keyDeviceDescriptor_t));
      ((macSecurityPibKeyDeviceEntry_t *)temp)->key_device_index = entry;
      ((macSecurityPibKeyDeviceEntry_t *)temp)->key_index = keyIndex;
      return (ZMacStatus_t) MAC_MlmeSetSecurityReq( attr, temp );
      break;
      
    case ZMacKeyUsageEntry:
      keyIndex = *ptr++; /* key_index */
      entry    = *ptr++; /* key_key_usage_index */
      
      /* Transform byte array to propriety PIB struct */
      osal_memcpy(tmp, ptr, sizeof(keyUsageDescriptor_t));
      osal_memcpy(&((macSecurityPibKeyUsageEntry_t *)temp)->macKeyUsageEntry, tmp, sizeof(keyUsageDescriptor_t));
      ((macSecurityPibKeyUsageEntry_t *)temp)->key_key_usage_index = entry;
      ((macSecurityPibKeyUsageEntry_t *)temp)->key_index = keyIndex;
      return (ZMacStatus_t) MAC_MlmeSetSecurityReq( attr, temp );
      break;
      
    case ZMacKeyEntry:
      keyIndex = *ptr++; /* key_index */
      
      /* Transform byte array to propriety PIB struct */
      osal_memcpy(tmp, ptr, MAC_KEY_MAX_LEN);
      osal_memcpy(&((macSecurityPibKeyEntry_t *)temp)->keyEntry, tmp, MAC_KEY_MAX_LEN);
      ptr+=MAC_KEY_MAX_LEN;
      
      osal_memcpy(&frameCounter, ptr, sizeof(frameCounter));
      
      ((macSecurityPibKeyEntry_t *)temp)->frameCounter = frameCounter;
      ((macSecurityPibKeyEntry_t *)temp)->key_index = keyIndex;
      return (ZMacStatus_t) MAC_MlmeSetSecurityReq( attr, temp );
      break;
      
    case ZMacDeviceEntry:
       entry = *ptr++;
       
       /* Transform byte array to propriety PIB struct */
       osal_memcpy(tmp, ptr, sizeof(deviceDescriptor_t));
       osal_memcpy(&((macSecurityPibDeviceEntry_t *)temp)->macDeviceEntry, tmp, sizeof(deviceDescriptor_t));
   
      ((macSecurityPibDeviceEntry_t *)temp)->device_index = entry;
      return (ZMacStatus_t) MAC_MlmeSetSecurityReq( attr, temp );
      break;

    case ZMacSecurityLevelEntry:
      entry = *ptr++; /* security_level_index */
      
      /* Transform byte array to propriety PIB struct */
      osal_memcpy(tmp, ptr, sizeof(securityLevelDescriptor_t));
      osal_memcpy(&((macSecurityPibSecurityLevelEntry_t *)temp)->macSecurityLevelEntry, tmp, sizeof(securityLevelDescriptor_t));
      
      ((macSecurityPibSecurityLevelEntry_t *)temp)->security_level_index = entry;
      return (ZMacStatus_t) MAC_MlmeSetSecurityReq( attr, temp );
      break;
  }

//.........这里部分代码省略.........

/*********************************************************************
 * @brief   Get a GAP Role parameter.
 *
 * Public function defined in broadcaster.h.
 */
bStatus_t GAPRole_GetParameter( uint16 param, void *pValue )
{
  bStatus_t ret = SUCCESS;
  switch ( param )
  {
    case GAPROLE_PROFILEROLE:
      *((uint8*)pValue) = gapRole_profileRole;
      break;

    case GAPROLE_BD_ADDR:
      VOID osal_memcpy( pValue, gapRole_bdAddr, B_ADDR_LEN ) ;
      break;

    case GAPROLE_ADVERT_ENABLED:
      *((uint8*)pValue) = gapRole_AdvEnabled;
      break;

    case GAPROLE_ADVERT_OFF_TIME:
      *((uint16*)pValue) = gapRole_AdvertOffTime;
      break;

    case GAPROLE_ADVERT_DATA:
      VOID osal_memcpy( pValue , gapRole_AdvertData, gapRole_AdvertDataLen );
      break;

    case GAPROLE_SCAN_RSP_DATA:
      VOID osal_memcpy( pValue, gapRole_ScanRspData, gapRole_ScanRspDataLen ) ;
      break;

    case GAPROLE_ADV_EVENT_TYPE:
      *((uint8*)pValue) = gapRole_AdvEventType;
      break;

    case GAPROLE_ADV_DIRECT_TYPE:
      *((uint8*)pValue) = gapRole_AdvDirectType;
      break;

    case GAPROLE_ADV_DIRECT_ADDR:
      VOID osal_memcpy( pValue, gapRole_AdvDirectAddr, B_ADDR_LEN ) ;
      break;

    case GAPROLE_ADV_CHANNEL_MAP:
      *((uint8*)pValue) = gapRole_AdvChanMap;
      break;

    case GAPROLE_ADV_FILTER_POLICY:
      *((uint8*)pValue) = gapRole_AdvFilterPolicy;
      break;

    default:
      // The param value isn't part of this profile, try the GAP.
      if ( param < TGAP_PARAMID_MAX )
      {
        *((uint16*)pValue) = GAP_GetParamValue( param );
      }
      else
      {
        ret = INVALIDPARAMETER;
      }
      break;
  }

  return ( ret );
}

/*********************************************************************
 * @fn      oadManagerEventCB
 *
 * @brief   Central event callback function.
 *
 * @param   pEvent - pointer to event structure
 *
 * @return  none
 */
static void oadManagerEventCB( gapCentralRoleEvent_t *pEvent )
{
  switch ( pEvent->gap.opcode )
  {
    case GAP_DEVICE_INIT_DONE_EVENT:
      {
        VOID osal_memcpy( oadManagerAddr, pEvent->initDone.devAddr, B_ADDR_LEN );

#if (defined HAL_LCD) && (HAL_LCD == TRUE)
        LCD_WRITE_STRING( "OAD Manager", HAL_LCD_LINE_1 );
        LCD_WRITE_STRING( bdAddr2Str( oadManagerAddr ),  HAL_LCD_LINE_2 );
#endif
      }
      break;

    case GAP_DEVICE_INFO_EVENT:
      {
        // if filtering device discovery results based on service UUID
        if ( DEFAULT_DEV_DISC_BY_SVC_UUID == TRUE )
        {
          if ( oadManagerFindServUUID( pEvent->deviceInfo.pEvtData, pEvent->deviceInfo.dataLen ) )
          {
            oadManagerAddDeviceInfo( pEvent->deviceInfo.addr, pEvent->deviceInfo.addrType );
          }
        }
      }
      break;

    case GAP_DEVICE_DISCOVERY_EVENT:
      {
        // discovery complete
        oadManagerScanning = FALSE;

        // if not filtering device discovery results based on service UUID
        if ( DEFAULT_DEV_DISC_BY_SVC_UUID == FALSE )
        {
          // Copy results
          oadManagerScanRes = pEvent->discCmpl.numDevs;
          osal_memcpy( oadManagerDevList, pEvent->discCmpl.pDevList,
                       (sizeof( gapDevRec_t ) * pEvent->discCmpl.numDevs) );
        }

#if (defined HAL_LCD) && (HAL_LCD == TRUE)
        LCD_WRITE_STRING_VALUE( "Devices Found", oadManagerScanRes, 10, HAL_LCD_LINE_1 );
        if ( oadManagerScanRes > 0 )
        {
          LCD_WRITE_STRING( "<- To Select", HAL_LCD_LINE_2 );
        }
#endif
        // initialize scan index to last device
        oadManagerScanIdx = oadManagerScanRes;
      }
      break;

    case GAP_LINK_ESTABLISHED_EVENT:
      {
        if ( pEvent->gap.hdr.status == SUCCESS )
        {
          oadManagerState = BLE_STATE_CONNECTED;
          oadManagerConnHandle = pEvent->linkCmpl.connectionHandle;

          (void)osal_set_event(oadManagerTaskId, CONN_INTERVAL_EVT);

#if (defined HAL_LCD) && (HAL_LCD == TRUE)
          LCD_WRITE_STRING( "Connected", HAL_LCD_LINE_1 );
          LCD_WRITE_STRING( "", HAL_LCD_LINE_3 );
#endif
        }
        else
        {
          oadManagerState = BLE_STATE_IDLE;
          oadManagerConnHandle = GAP_CONNHANDLE_INIT;
        }
      }
      break;

    case GAP_LINK_TERMINATED_EVENT:
      {
        oadManagerState = BLE_STATE_IDLE;
        oadManagerConnHandle = GAP_CONNHANDLE_INIT;

#if (defined HAL_LCD) && (HAL_LCD == TRUE)
        LCD_WRITE_STRING( "OAD Manager", HAL_LCD_LINE_1 );
        LCD_WRITE_STRING( bdAddr2Str( oadManagerAddr ),  HAL_LCD_LINE_2 );
        LCD_WRITE_STRING( "Disconnected", HAL_LCD_LINE_3 );
#endif
      }
      break;

    default:
      break;
//.........这里部分代码省略.........

/*=================================================================================================
 * @fn          rxAddrIsr
 *
 * @brief       Receive ISR state for decoding address.  Reads and stores the address information
 *              from the incoming packet.
 *
 * @param       none
 *
 * @return      none
 *=================================================================================================
 */
static void rxAddrIsr(void)
{
  uint8 buf[MAX_ADDR_FIELDS_LEN];
  uint8 dstAddrMode;
  uint8 srcAddrMode;
#ifdef FEATURE_MAC_SECURITY
  uint8 securityControl;
#endif /* MAC_SECURITY */
  uint8  * p;

  MAC_ASSERT(rxNextLen != 0); /* logic assumes at least one address byte in buffer */

  /*  read out address fields into local buffer in one shot */
  MAC_RADIO_READ_RX_FIFO(buf, rxNextLen);

  /* set pointer to buffer with addressing fields */
  p = buf;

  /* destination address */
  dstAddrMode = MAC_DEST_ADDR_MODE(&rxBuf[1]);
  if (dstAddrMode != SADDR_MODE_NONE)
  {
    pRxBuf->mac.srcPanId = pRxBuf->mac.dstPanId = BUILD_UINT16(p[0], p[1]);
    p += MAC_PAN_ID_FIELD_LEN;
    if (dstAddrMode == SADDR_MODE_EXT)
    {
      sAddrExtCpy(pRxBuf->mac.dstAddr.addr.extAddr, p);
      p += MAC_EXT_ADDR_FIELD_LEN;
    }
    else
    {
      pRxBuf->mac.dstAddr.addr.shortAddr = BUILD_UINT16(p[0], p[1]);
      p += MAC_SHORT_ADDR_FIELD_LEN;
    }
  }

  /* sources address */
  srcAddrMode = MAC_SRC_ADDR_MODE(&rxBuf[1]);
  if (srcAddrMode != SADDR_MODE_NONE)
  {
    if (!(pRxBuf->internal.flags & MAC_RX_FLAG_INTRA_PAN))
    {
      pRxBuf->mac.srcPanId = BUILD_UINT16(p[0], p[1]);
      p += MAC_PAN_ID_FIELD_LEN;
    }
    if (srcAddrMode == SADDR_MODE_EXT)
    {
      sAddrExtCpy(pRxBuf->mac.srcAddr.addr.extAddr, p);
    }
    else
    {
      pRxBuf->mac.srcAddr.addr.shortAddr = BUILD_UINT16(p[0], p[1]);
    }
  }

#ifdef FEATURE_MAC_SECURITY
  if (MAC_SEC_ENABLED(&rxBuf[1]))
  {
    uint8 keyIdMode;

    if (MAC_FRAME_VERSION(&rxBuf[1]) == 0)
    {
      /* MAC_UNSUPPORTED_LEGACY - Cancel the outgoing TX ACK.
       * It may be too late but we have to try.
       */
      MAC_RADIO_CANCEL_TX_ACK();

      /* clean up after unsupported security legacy */
      macRxHaltCleanup();
      return;
    }

    /* Copy addressing fields to RX buffer */
    osal_memcpy(pRxBuf->mhr.p, buf, rxNextLen);
    pRxBuf->mhr.p   += rxNextLen;
    pRxBuf->mhr.len += rxNextLen;

    /*-------------------------------------------------------------------------------
     *  Prepare for auxiliary security header interrupts.
     */

    /* read out security control field from FIFO (threshold set so bytes are guaranteed to be there) */
    MAC_RADIO_READ_RX_FIFO(&securityControl, MAC_SEC_CONTROL_FIELD_LEN);

    /* Copy security fields to MHR buffer */
    *pRxBuf->mhr.p   = securityControl;
    pRxBuf->mhr.p   += MAC_SEC_CONTROL_FIELD_LEN;
    pRxBuf->mhr.len += MAC_SEC_CONTROL_FIELD_LEN;

//.........这里部分代码省略.........

/***************************************************************************************************
 * @fn      Uart_Msg_Parse
 *
 * @brief   parse uart Msg,this msg was came from e009 uart
 *
 * @param   pBuffer: msg; length: msg length
 *
 * @return  None
 ***************************************************************************************************/
bool Uart_Msg_Parse(uint8* pBuffer, uint8 length)
{
    bool ret = TRUE;
    
    // pBuffer[0] = MSG_IDENTIFICATION;pBuffer[1]=msd id
    switch(pBuffer[1])
    {
        case ID_E0092BLE_G_RSP:
		    last_uart_send.msg_buffer[0] = 0;
            break;
			
        case ID_E0092BLE_TIME:
            BLE_Common_Response(ID_E0092BLE_TIME,RSP_OK);
		    uart_pulse_timeout_count = 0;
            Set_Ble_Time(pBuffer+3); 
            break;
			
        case ID_E0092BLE_REBOOT_BLE: 
            BLE_Common_Response(ID_E0092BLE_REBOOT_BLE,RSP_OK);
            BLE_Reboot();
            break;
			
        case ID_E0092BLE_IMEI:
            BLE_Common_Response(ID_E0092BLE_IMEI,RSP_OK);
			//gE009_ready = true;
            //osal_start_timerEx( simpleBLETaskId, SBP_E009_READY, 250 );
			osal_memcpy(gSerial_Num[DEV_A].imei,pBuffer+3,SERIAl_NUM_LEN);
            break;
			
        case ID_E0092BLE_IMSI:
            BLE_Common_Response(ID_E0092BLE_IMSI,RSP_OK);
			gE009_ready = true;
            osal_start_timerEx( simpleBLETaskId, SBP_E009_READY, 250 );
			osal_memcpy(gSerial_Num[DEV_A].imsi,pBuffer+3,SERIAl_NUM_LEN);
            break;         
           
         case ID_E0092BLE_BAT:     //QST E009 to BLE
            BLE_Common_Response(ID_E0092BLE_BAT,RSP_OK);
			//sys_config.battery = *(pBuffer+3);
            break;

        case ID_E0092BLE_START_E009:
            BLE_Common_Response(ID_E0092BLE_START_E009,RSP_OK);
			BLE_Powerup_QST_E009(true);
            break;  
			
        case ID_E0092BLE_POWEROFF:  //QST E009 to BLE,power off himself
            BLE_Common_Response(ID_E0092BLE_POWEROFF,RSP_OK);
			BLE_Powerup_QST_E009(false);
            break;  
			
        case ID_E0092BLE_REBOOT_OTHER:  //HWJ reboot QST, QST reboot HWJ
            BLE_Common_Response(ID_E0092BLE_REBOOT_OTHER,RSP_OK);
            BLE_Reboot_Other();
            break;   
			
        case ID_E0092BLE_STATE:
			E009_state = *(pBuffer+3);
			if(E009_state == E009_STATE_SLEEP)
			{
				Save_E009_powerup_time( pBuffer+4 );
			}
			BLE_Common_Response(ID_E0092BLE_STATE,E009_state);
            break;   			
			
        case ID_E0092BLE_START_ADV:    //QST E009 to BLE, start advertising
            BLE_Common_Response(ID_E0092BLE_START_ADV,RSP_OK);
            BLE_start_adv();
            break;   	
			
        case ID_E0092BLE_PAR:
            BLE_Common_Response(ID_E0092BLE_PAR,RSP_OK);
			BLE_set_par( pBuffer+3 );
            break;  
			
        case ID_E0092BLE_REBOOT:
            BLE_Common_Response(ID_E0092BLE_REBOOT,RSP_OK);
			BLE_Reboot_E009();
            break;  
            
        default:    
            ret = FALSE;            
            break;
    }                 
    
    return ret;
}

/*********************************************************************
 * @fn      SampleApp_Init
 *
 * @brief   Initialization function for the Generic App Task.
 *          This is called during initialization and should contain
 *          any application specific initialization (ie. hardware
 *          initialization/setup, table initialization, power up
 *          notificaiton ... ).
 *
 * @param   task_id - the ID assigned by OSAL.  This ID should be
 *                    used to send messages and set timers.
 *
 * @return  none
 */
void SampleApp_Init( uint8 task_id )
{
  macRadioSetTxPower(20);
  SampleApp_TaskID = task_id;
  SampleApp_NwkState = DEV_INIT;
  SampleApp_TransID = 0;

   /***********串口初始化****************/
  MT_UartInit(); //串口配置初始化
  MT_UartRegisterTaskID(task_id);//登记串口任务号
  HalUARTWrite(0,"Hello World\r\n",13); // (串口 0,'字符',字符个数)
  // Device hardware initialization can be added here or in main() (Zmain.c).
  // If the hardware is application specific - add it here.
  // If the hardware is other parts of the device add it in main().

 #if defined ( BUILD_ALL_DEVICES )
  // The "Demo" target is setup to have BUILD_ALL_DEVICES and HOLD_AUTO_START
  // We are looking at a jumper (defined in SampleAppHw.c) to be jumpered
  // together - if they are - we will start up a coordinator. Otherwise,
  // the device will start as a router.
  if ( readCoordinatorJumper() )
    zgDeviceLogicalType = ZG_DEVICETYPE_COORDINATOR;
  else
    zgDeviceLogicalType = ZG_DEVICETYPE_ROUTER;
#endif // BUILD_ALL_DEVICES

#if defined ( HOLD_AUTO_START )
  // HOLD_AUTO_START is a compile option that will surpress ZDApp
  //  from starting the device and wait for the application to
  //  start the device.
  ZDOInitDevice(0);
#endif

  // Setup for the periodic message's destination address
  // Broadcast to everyone
  SampleApp_Periodic_DstAddr.addrMode = (afAddrMode_t)AddrBroadcast;
  SampleApp_Periodic_DstAddr.endPoint = SAMPLEAPP_ENDPOINT;
  SampleApp_Periodic_DstAddr.addr.shortAddr = 0xFFFF;
  
  // Setup for the flash command's destination address - Group 1
  SampleApp_Flash_DstAddr.addrMode = (afAddrMode_t)afAddrGroup;
  SampleApp_Flash_DstAddr.endPoint = SAMPLEAPP_ENDPOINT;
  SampleApp_Flash_DstAddr.addr.shortAddr = SAMPLEAPP_FLASH_GROUP;

  // Fill out the endpoint description.
  SampleApp_epDesc.endPoint = SAMPLEAPP_ENDPOINT;
  SampleApp_epDesc.task_id = &SampleApp_TaskID;
  SampleApp_epDesc.simpleDesc
            = (SimpleDescriptionFormat_t *)&SampleApp_SimpleDesc;
  SampleApp_epDesc.latencyReq = noLatencyReqs;
   
  // Register the endpoint description with the AF
  afRegister( &SampleApp_epDesc );
  
  // Register for all key events - This app will handle all key events
  RegisterForKeys( SampleApp_TaskID );

  /*分组信息初始化*/
  SampleApp_Group.ID = 0x0001;
  osal_memcpy( SampleApp_Group.name, "Group 1", 7  );
  aps_AddGroup( SAMPLEAPP_ENDPOINT, &SampleApp_Group );

#if defined ( LCD_SUPPORTED )
  HalLcdWriteString( "SampleApp", HAL_LCD_LINE_1 );
#endif
  
}

//.........这里部分代码省略.........
          if( MAC_SrcMatchCheckAllPending() == MAC_AUTOACK_PENDING_ALL_ON )
          {
            ackWithPending = MAC_RX_FLAG_ACK_PENDING;
          }
        }
        else
        {
          /* When autopend is disabled, check the application pending callback */
          if( macRxCheckPendingCallback() )
          {
            ackWithPending = MAC_RX_FLAG_ACK_PENDING;
          }
        }
      }
    }

    if( ackWithPending == MAC_RX_FLAG_ACK_PENDING )
    {
      MAC_RADIO_TX_ACK_PEND();
    }
    else
    {
      MAC_RADIO_TX_ACK();
    }


    /* request a callback to macRxAckTxDoneCallback() when the ACK transmit has finished */
    MAC_RADIO_REQUEST_ACK_TX_DONE_CALLBACK();
    HAL_EXIT_CRITICAL_SECTION(s);
  }

 /*-------------------------------------------------------------------------------
  *  Populate the receive buffer going up to high-level.
  */

  /* configure the payload buffer
   * save MAC header pointer regardless of security status.
   */
  pRxBuf->mhr.p   = pRxBuf->msdu.p   = (uint8 *) (pRxBuf + 1);
  pRxBuf->mhr.len = pRxBuf->msdu.len =  rxPayloadLen;

  if (MAC_SEC_ENABLED(&rxBuf[1]))
  {
    /* Copy FCF and sequence number to RX buffer */
    pRxBuf->mhr.len = MAC_FCF_FIELD_LEN + MAC_SEQ_NUM_FIELD_LEN;
    osal_memcpy(pRxBuf->mhr.p, &rxBuf[1], pRxBuf->mhr.len);
    pRxBuf->mhr.p += pRxBuf->mhr.len;
  }

  /* set internal values */
  pRxBuf->mac.srcAddr.addrMode  = srcAddrMode;
  pRxBuf->mac.dstAddr.addrMode  = dstAddrMode;
  pRxBuf->mac.timestamp         = MAC_RADIO_BACKOFF_CAPTURE();
  pRxBuf->mac.timestamp2        = MAC_RADIO_TIMER_CAPTURE();

  /* Special Case for Enhanced Beacon Request which has a different
   * frame version
   */
#ifdef FEATURE_ENHANCED_BEACON
  if( MAC_FRAME_VERSION(&rxBuf[1]) == 2 )
  {
      pRxBuf->internal.frameType  = MAC_FRAME_TYPE_INTERNAL_MAC_VERSION_E | \
                                  MAC_FRAME_TYPE(&rxBuf[1]);
  }
  else
#endif
  {
    pRxBuf->internal.frameType  = MAC_FRAME_TYPE(&rxBuf[1]);
  }

  pRxBuf->mac.dsn               = MAC_SEQ_NUMBER(&rxBuf[1]);
  pRxBuf->internal.flags        = INTERNAL_FCF_FLAGS(&rxBuf[1]) | ackWithPending;

  /*-------------------------------------------------------------------------------
   *  If the processing the addressing fields does not require more bytes from
   *  the FIFO go directly address processing function.  Otherwise, configure
   *  interrupt to jump there once bytes are received.
   */
  if (addrLen == 0)
  {
    /* no addressing fields to read, prepare for payload interrupts */
    pFuncRxState = &rxPayloadIsr;
    rxPrepPayload();
  }
  else
  {
    /* need to read and process addressing fields, prepare for address interrupt */
    rxNextLen = addrLen;
    if (MAC_SEC_ENABLED(&rxBuf[1]))
    {
      /* When security is enabled, read off security control field as well */
      MAC_RADIO_SET_RX_THRESHOLD(rxNextLen + MAC_SEC_CONTROL_FIELD_LEN);
    }
    else
    {
      MAC_RADIO_SET_RX_THRESHOLD(rxNextLen);
    }
    pFuncRxState = &rxAddrIsr;
  }
}

/**************************************************************************************************
 * @fn          MAC_MlmeSetReq
 *
 * @brief       This direct execute function sets an attribute value
 *              in the MAC PIB.
 *
 * input parameters
 *
 * @param       pibAttribute - The attribute identifier.
 * @param       pValue - pointer to the attribute value.
 *
 * output parameters
 *
 * None.
 *
 * @return      The status of the request, as follows:
 *              MAC_SUCCESS Operation successful.
 *              MAC_UNSUPPORTED_ATTRIBUTE Attribute not found.
 *
 **************************************************************************************************
 */
uint8 MAC_MlmeSetReq(uint8 pibAttribute, void *pValue)
{
  uint8         i;
  halIntState_t intState;

  if (pibAttribute == MAC_BEACON_PAYLOAD)
  {
    pMacPib->pBeaconPayload = pValue;
    return MAC_SUCCESS;
  }

  /* look up attribute in PIB table */
  if ((i = MAP_macPibIndex(pibAttribute)) == MAC_PIB_INVALID)
  {
    return MAC_UNSUPPORTED_ATTRIBUTE;
  }

  /* do range check; no range check if min and max are zero */
  if ((macPibTbl[i].min != 0) || (macPibTbl[i].max != 0))
  {
    /* if min == max, this is a read-only attribute */
    if (macPibTbl[i].min == macPibTbl[i].max)
    {
      return MAC_READ_ONLY;
    }

    /* check for special cases */
    if (pibAttribute == MAC_MAX_FRAME_TOTAL_WAIT_TIME)
    {
      if ((*((uint16 *) pValue) < MAC_MAX_FRAME_RESPONSE_MIN) ||
          (*((uint16 *) pValue) > MAC_MAX_FRAME_RESPONSE_MAX))
      {
        return MAC_INVALID_PARAMETER;
      }
    }

    /* range check for general case */
    if ((*((uint8 *) pValue) < macPibTbl[i].min) || (*((uint8 *) pValue) > macPibTbl[i].max))
    {
      return MAC_INVALID_PARAMETER;
    }

  }

  /* set value in PIB */
  HAL_ENTER_CRITICAL_SECTION(intState);
  osal_memcpy((uint8 *) pMacPib + macPibTbl[i].offset, pValue, macPibTbl[i].len);
  HAL_EXIT_CRITICAL_SECTION(intState);

  /* handle special cases */
  switch (pibAttribute)
  {
    case MAC_PAN_ID:
      /* set pan id in radio */
      macRadioSetPanID(pMacPib->panId);
      break;

    case MAC_SHORT_ADDRESS:
      /* set short address in radio */
      macRadioSetShortAddr(pMacPib->shortAddress);
      break;

    case MAC_RX_ON_WHEN_IDLE:
      /* turn rx on or off */
      if (pMacPib->rxOnWhenIdle)
      {
        macRxEnable(MAC_RX_WHEN_IDLE);
      }
      else
      {
        macRxDisable(MAC_RX_WHEN_IDLE);
      }
      break;

    case MAC_LOGICAL_CHANNEL:
      macRadioSetChannel(pMacPib->logicalChannel);
      break;

    case MAC_EXTENDED_ADDRESS:
//.........这里部分代码省略.........

/* wcn_compressor_out - get the result data from L2 buffer
 * @ cprs - compressor's pointer
 * @ pbuf - point to L2 buffer
 * @ plen - out len 
 *
 * Retunr 0 if success, else NULL
 */
INT32 wcn_compressor_out(P_WCN_COMPRESSOR_T cprs, PUINT8 *pbuf, PINT32 plen)
{
    INT32 ret = 0;
    INT32 tmp_len = 0;

    if ((!cprs) || (!pbuf) || (!plen)) {
        STP_DBG_ERR_FUNC("invalid para!\n");
        return -1;
    }

    // check if there's L1 data need flush to L2 buffer 
    if (cprs->L1_pos > 0) {
        tmp_len = cprs->L2_buf_sz - cprs->L2_pos;
        
        if (cprs->f_compress_en && cprs->handler) {
            // need compress
            ret = cprs->handler(cprs->worker, cprs->L1_buf, cprs->L1_pos, &cprs->L2_buf[cprs->L2_pos], &tmp_len, 1);

            if (!ret) {
		cprs->crc32 = (crc32(cprs->crc32, cprs->L1_buf, cprs->L1_pos));
                cprs->L2_pos += tmp_len;

		/* Add 8 byte suffix
		   ===
		   32 bits UNCOMPRESS SIZE
		   32 bits CRC
		*/
		*(uint32_t *)(&cprs->L2_buf[cprs->L2_pos]) = (cprs->crc32 ^ 0xffffffffUL);
		*(uint32_t *)(&cprs->L2_buf[cprs->L2_pos + 4]) = cprs->uncomp_size;
		cprs->L2_pos += 8;

                STP_DBG_INFO_FUNC("compress OK!\n");
            } else {
                STP_DBG_ERR_FUNC("compress error!\n");
            }
        } else {
            // no need compress
            tmp_len = (cprs->L1_pos > tmp_len) ? tmp_len : cprs->L1_pos;
            osal_memcpy(&cprs->L2_buf[cprs->L2_pos], cprs->L1_buf, tmp_len);
            cprs->L2_pos += tmp_len;
        }
        
        cprs->L1_pos = 0;
    }


    *pbuf = cprs->L2_buf;
    *plen = cprs->L2_pos;

    STP_DBG_INFO_FUNC("0x%08x, len %d\n", (unsigned int)*pbuf, *plen);
    
#if 1
    ret = zlib_deflateReset((z_stream*)cprs->worker);
    if (ret != Z_OK) {
        STP_DBG_ERR_FUNC("reset failed!\n");
        return -2;
    }
#endif

    return 0;
}

/* wcn_compressor_in - put in a raw data, and compress L1 buffer if need
 * @ cprs - compressor's pointer
 * @ buf - raw data buffer
 * @ len - raw data length
 * @ finish - core dump finish or not, 1: finished; 0: not finish
 *
 * Retunr 0 if success, else NULL
 */
INT32 wcn_compressor_in(P_WCN_COMPRESSOR_T cprs, PUINT8 buf, INT32 len, INT32 finish)
{
    INT32 tmp_len = 0;
    INT32 ret = 0;
    
    if (!cprs) {
        STP_DBG_ERR_FUNC("invalid para!\n");
        return -1;
    }
    
    cprs->uncomp_size += len;

    // check L1 buf valid space
    if (len > (cprs->L1_buf_sz - cprs->L1_pos)) {
        STP_DBG_INFO_FUNC("L1 buffer full\n");
        
        if (cprs->f_compress_en && cprs->handler) {
            // need compress
            // compress L1 buffer, and put result to L2 buffer
            tmp_len = cprs->L2_buf_sz - cprs->L2_pos;
            ret = cprs->handler(cprs->worker, cprs->L1_buf, cprs->L1_pos, &cprs->L2_buf[cprs->L2_pos], &tmp_len, finish);
            if (!ret) {
		cprs->crc32 = (crc32(cprs->crc32, cprs->L1_buf, cprs->L1_pos));
                cprs->L2_pos += tmp_len;

		if (finish) {
			/* Add 8 byte suffix
			   ===
			   32 bits UNCOMPRESS SIZE
			   32 bits CRC
			*/
			*(uint32_t *)(&cprs->L2_buf[cprs->L2_pos]) = (cprs->crc32 ^ 0xffffffffUL);
			*(uint32_t *)(&cprs->L2_buf[cprs->L2_pos + 4]) = cprs->uncomp_size;
			cprs->L2_pos += 8;
		}
                STP_DBG_INFO_FUNC("compress OK!\n");
            } else {
                STP_DBG_ERR_FUNC("compress error!\n");
            }
        } else {
            // no need compress
            // Flush L1 buffer to L2 buffer
            STP_DBG_INFO_FUNC("No need do compress, Put to L2 buf\n");
            
            tmp_len = cprs->L2_buf_sz - cprs->L2_pos;
            tmp_len = (cprs->L1_pos > tmp_len) ? tmp_len : cprs->L1_pos;
            osal_memcpy(&cprs->L2_buf[cprs->L2_pos], cprs->L1_buf, tmp_len);
            cprs->L2_pos += tmp_len;
        }
            
        // reset L1 buf pos
        cprs->L1_pos = 0;

        // put curren data to L1 buf
        if (len > cprs->L1_buf_sz) {
            STP_DBG_ERR_FUNC("len=%d, too long err!\n", len);
        } else {
            STP_DBG_INFO_FUNC("L1 Flushed, and Put %d bytes to L1 buf\n", len);
            osal_memcpy(&cprs->L1_buf[cprs->L1_pos], buf, len);
            cprs->L1_pos += len;
        }
    } else {
        // put to L1 buffer
        STP_DBG_INFO_FUNC("Put %d bytes to L1 buf\n", len);
        
        osal_memcpy(&cprs->L1_buf[cprs->L1_pos], buf, len);
        cprs->L1_pos += len;
    }

    return ret;
}

/*********************************************************************
 * @fn      SimpleProfile_SetParameter
 *
 * @brief   Set a Simple Profile parameter.
 *
 * @param   param - Profile parameter ID
 * @param   len - length of data to right
 * @param   value - pointer to data to write.  This is dependent on
 *          the parameter ID and WILL be cast to the appropriate 
 *          data type (example: data type of uint16 will be cast to 
 *          uint16 pointer).
 *
 * @return  bStatus_t
 */
bStatus_t SimpleProfile_SetParameter( uint8 param, uint8 len, void *value )
{
  bStatus_t ret = SUCCESS;
  switch ( param )
  {
    case SIMPLEPROFILE_CHAR1:
      if ( len == sizeof ( uint8 ) ) 
      {
        simpleProfileChar1 = *((uint8*)value);
      }
      else
      {
        ret = bleInvalidRange;
      }
      break;

    case SIMPLEPROFILE_CHAR2:
      if ( len == sizeof ( uint8 ) ) 
      {
        simpleProfileChar2 = *((uint8*)value);
      }
      else
      {
        ret = bleInvalidRange;
      }
      break;

    case SIMPLEPROFILE_CHAR3:
      if ( len == sizeof ( uint8 ) ) 
      {
        simpleProfileChar3 = *((uint8*)value);
      }
      else
      {
        ret = bleInvalidRange;
      }
      break;

    case SIMPLEPROFILE_CHAR4:
      if ( len == sizeof ( uint8 ) ) 
      {
        simpleProfileChar4 = *((uint8*)value);
        
        // See if Notification/Indication has been enabled
        simpleProfile_ProcessCharCfg( simpleProfileChar4Config, &simpleProfileChar4, FALSE );
        
      }
      else
      {
        ret = bleInvalidRange;
      }
      break;

    case SIMPLEPROFILE_CHAR5:
      if ( len == SIMPLEPROFILE_CHAR5_LEN ) 
      {
        VOID osal_memcpy( simpleProfileChar5, value, SIMPLEPROFILE_CHAR5_LEN );
      }
      else
      {
        ret = bleInvalidRange;
      }
      break;
      
    default:
      ret = INVALIDPARAMETER;
      break;
  }
  
  return ( ret );
}