CC2540/CC2541 : Change Scan Response Data( SRP) dynamically

#include "bcomdef.h"
#include "OSAL.h"
#include "OSAL_PwrMgr.h"

#include "OnBoard.h"
#include "hal_uart.h"
#include "hal_lcd.h"
#include "serialcommunication.h"

#if defined ( PLUS_BROADCASTER )
  #include "peripheralBroadcaster.h"
#else
  #include "peripheral.h"
#endif

void SerialInitTransport(void)
{
  halUARTCfg_t uartConfig;

  // configure UART
  uartConfig.configured           = TRUE;
  uartConfig.baudRate             = SBP_UART_BR;
  uartConfig.flowControl          = SBP_UART_FC;
  uartConfig.flowControlThreshold = SBP_UART_FC_THRESHOLD;//enable when flowControl is valid
  uartConfig.rx.maxBufSize        = SBP_UART_RX_BUF_SIZE;//UART RX 
  uartConfig.tx.maxBufSize        = SBP_UART_TX_BUF_SIZE;//UART TX
  uartConfig.idleTimeout          = SBP_UART_IDLE_TIMEOUT;
  uartConfig.intEnable            = SBP_UART_INT_ENABLE;//interrupt enable
  uartConfig.callBackFunc         = sbpSerialAppCallback;//uart callback function

  // start UART
  // Note: Assumes no issue opening UART port.
  (void)HalUARTOpen( SBP_UART_PORT, &uartConfig );

  return;
}/*serialAppInitTransport*/


uint16 numBytes;

void sbpSerialAppCallback(uint8 port, uint8 event)
{
  uint8  pktBuffer[SBP_UART_RX_BUF_SIZE];
  // unused input parameter; PC-Lint error 715.
  (void)event;
  HalLcdWriteString("Data form my UART:", HAL_LCD_LINE_4 );
  //return lengh
  if( (numBytes = Hal_UART_RxBufLen(port)) > 0)
  {      
      //int j;
        //read all data
 (void)HalUARTRead(port, pktBuffer, numBytes);
        //uint8 blankStr[HAL_LCD_MAX_CHARS];
       // memset(&blankStr[0], 32, HAL_LCD_MAX_CHARS);
                      
 HalLcdWriteString(pktBuffer, HAL_LCD_LINE_5 );
        
        //GAPRole_SetParameter( GAPROLE_SCAN_RSP_DATA, 
        //  numBytes, &pktBuffer[0] );
        
        //simpleProfileChar1 = pktBuffer[0];
  }/*if */
  
}/*sbpSerialAppCallback*/


static uint8 sendMsgTo_TaskID;

void SerialApp_Init( uint8 taskID )
{
  SerialInitTransport(); 
  sendMsgTo_TaskID = taskID; //save task id, for spare using.
}/*SerialApp_Init*/

#ifndef _SERIAL_COMMUNICATION_H_
#define _SERIAL_COMMUNICATION_H_

#ifdef __cplusplus
extern "C"
{
#endif
  

#define SBP_UART_PORT                  HAL_UART_PORT_0
//#define SBP_UART_FC                    TRUE
#define SBP_UART_FC                    FALSE
#define SBP_UART_FC_THRESHOLD          48
#define SBP_UART_RX_BUF_SIZE           128
#define SBP_UART_TX_BUF_SIZE           128
#define SBP_UART_IDLE_TIMEOUT          6
#define SBP_UART_INT_ENABLE            TRUE
#define SBP_UART_BR                     HAL_UART_BR_57600 


// Serial Port Related
extern void SerialApp_Init(uint8 taskID);
extern void sbpSerialAppCallback(uint8 port, uint8 event);
void SerialInitTransport();

#ifdef __cplusplus
}
#endif

#endif/*_SERIAL_COMMUNICATION_H_*/

#if defined FEATURE_OAD
  #include "oad.h"
  #include "oad_target.h"
#endif

#include "serialcommunication.h"
/*********************************************************************
 * MACROS
 */

void SimpleBLEPeripheral_Init( uint8 task_id )
{
  simpleBLEPeripheral_TaskID = task_id;

  SerialApp_Init(task_id);
  // Setup the GAP

uint16 numBytes;

void sbpSerialAppCallback(uint8 port, uint8 event)
{
  uint8 pktBuffer[SBP_UART_RX_BUF_SIZE];
  // unused input parameter; PC-Lint error 715.
  (void)event;
  HalLcdWriteString("Data form my UART:", HAL_LCD_LINE_4 );
  
  //return length
  if( 0 < (numBytes = Hal_UART_RxBufLen(port)))
  {      
      
 (void)HalUARTRead(port, pktBuffer, numBytes);        
        
        
        uint8 buffer[32];
        uint16 scanRspDataSize;
        
        memset(&buffer[0], 0, 32);                

        osal_snv_write(NV_MEM_ID, 32, &buffer[0]);
        
        SetRspData(&pData[0], numBytes, &buffer[0], &scanRspDataSize);              
        osal_snv_write(NV_MEM_ID, scanRspDataSize, &buffer[0]);                                  
        HAL_SYSTEM_RESET();          
  }/*if */
  
}/*sbpSerialAppCallback*/

void SetRspData(uint8 *pName, uint16 nameLen, 
                uint8 *pRspData, uint16 *pScanRspDataSize)
{       

  if(nameLen  > 31 - (2 + (5 + 1) + (1 + 2)) )
    nameLen =  31 - (2 + (5 + 1) + (1 + 2));
    
  pRspData[0] = nameLen + 1;
  pRspData[1] = GAP_ADTYPE_LOCAL_NAME_COMPLETE;
  memcpy(&pRspData[2], pName, nameLen); 
 
  int16  i;
  
  i = nameLen + 2; 
  pRspData[i++] = 0x05;
  pRspData[i++] = GAP_ADTYPE_SLAVE_CONN_INTERVAL_RANGE;
  pRspData[i++] = LO_UINT16( DEFAULT_DESIRED_MIN_CONN_INTERVAL );     
  pRspData[i++] = HI_UINT16( DEFAULT_DESIRED_MIN_CONN_INTERVAL );
  pRspData[i++] = LO_UINT16( DEFAULT_DESIRED_MAX_CONN_INTERVAL );     
  pRspData[i++] = HI_UINT16( DEFAULT_DESIRED_MAX_CONN_INTERVAL );
  
  pRspData[i++] = 0x02;
  pRspData[i++] =GAP_ADTYPE_POWER_LEVEL;
  pRspData[i++] = 0;       // 0dBm;    
  
  *pScanRspDataSize =  i;
}/*SetRspData*/

 // Set the GAP Role Parameters    
    
    GAPRole_SetParameter( GAPROLE_ADVERT_ENABLED, sizeof( uint8 ), &initial_advertising_enable );
    GAPRole_SetParameter( GAPROLE_ADVERT_OFF_TIME, sizeof( uint16 ), &gapRole_AdvertOffTime );
       
    uint8 defaultName[32];
    
    memset(&defaultName[0], 0, 32);
    sprintf((char*)&defaultName[0], "defaultRSP");
    
    uint8 buffer[32];    
    memset(&buffer[0], 0, 32);
      
    uint8 ret;       
   
    uint16 scanRspDataSize;
    scanRspDataSize = 0;  

    ret = osal_snv_read(NV_MEM_ID, 32 , &buffer[0]);
   
    if(SUCCESS == ret )  
    {      
      int i;      
      scanRspDataSize = 0;       
      scanRspDataSize = 1 + buffer[0];      
      i = 1 + buffer[0];
      scanRspDataSize += 1 + buffer[i];
      i += 1 + buffer[i];      
      scanRspDataSize += 1 + buffer[i];
    }/*if*/    
    
    if(NV_OPER_FAILED == ret)
    {
      memset(&buffer[0], 0, 31);      
      SetRspData(&defaultName[0], strlen((char*)&defaultName[0]), 
                 &buffer[0], &scanRspDataSize);      
      //ret = osal_snv_read(0xfe, 32 , &buffer[0]);
      //osal_snv_write(0xfe, sizeof(scanRspData), &scanRspData[0]);
    }/*if */
       
    
    GAPRole_SetParameter( GAPROLE_SCAN_RSP_DATA, scanRspDataSize, &buffer[0] );