File content as of revision 2:6d0f581488b6:

#include <stdbool.h>
#include <stdint.h>
#include <string.h>

#include "mbed.h"
#include "nRF51822n.h"

#include "nordic_common.h"
//#include "nrf.h"
#include "app_error.h"
#include "ble_hci.h"
#include "ble_gap.h"
#include "ble_advdata.h"
#include "ble_error_log.h"
#include "nrf_gpio.h"
#include "ble_srv_common.h"
#include "ble_conn_params.h"
#include "nrf51_bitfields.h"
#include "ble_bondmngr.h"
#include "app_timer.h"
#include "ble_radio_notification.h"
#include "ble_flash.h"
#include "ble_debug_assert_handler.h"
#include "pstorage.h"
#include "nrf_soc.h"
#include "softdevice_handler.h"

#include "debug.h"


#define DEVICE_NAME                     "ANCC"                                               /**< Name of device. Will be included in the advertising data. */
#define APP_ADV_INTERVAL                40                                                   /**< The advertising interval (in units of 0.625 ms. This value corresponds to 25 ms). */
#define APP_ADV_INTERVAL_SLOW           3200                                                 /**< Slow advertising interval (in units of 0.625 ms. This value corresponds to 2 seconds). */
#define APP_ADV_TIMEOUT_IN_SECONDS      180                                                  /**< The advertising timeout in units of seconds. */
#define ADV_INTERVAL_FAST_PERIOD        30                                                   /**< The duration of the fast advertising period (in seconds). */

#define APP_TIMER_PRESCALER             0                                                    /**< Value of the RTC1 PRESCALER register. */
#define APP_TIMER_MAX_TIMERS            2                                                    /**< Maximum number of simultaneously created timers. */
#define APP_TIMER_OP_QUEUE_SIZE         4                                                    /**< Size of timer operation queues. */

#define MIN_CONN_INTERVAL               MSEC_TO_UNITS(500, UNIT_1_25_MS)                     /**< Minimum acceptable connection interval (0.5 seconds). */
#define MAX_CONN_INTERVAL               MSEC_TO_UNITS(1000, UNIT_1_25_MS)                    /**< Maximum acceptable connection interval (1 second). */
#define SLAVE_LATENCY                   0                                                    /**< Slave latency. */
#define CONN_SUP_TIMEOUT                MSEC_TO_UNITS(4000, UNIT_10_MS)                      /**< Connection supervisory timeout (4 seconds). */

#define FIRST_CONN_PARAMS_UPDATE_DELAY  APP_TIMER_TICKS(20 * 1000, APP_TIMER_PRESCALER)      /**< Time from initiating event (connect or start of notification) to first time sd_ble_gap_conn_param_update is called (20 seconds). */
#define NEXT_CONN_PARAMS_UPDATE_DELAY   APP_TIMER_TICKS(5 * 1000, APP_TIMER_PRESCALER)       /**< Time between each call to sd_ble_gap_conn_param_update after the first (5 seconds). */
#define MAX_CONN_PARAMS_UPDATE_COUNT    3                                                    /**< Number of attempts before giving up the connection parameter negotiation. */


#define SEC_PARAM_TIMEOUT               30                                                   /**< Timeout for Pairing Request or Security Request (in seconds). */
#define SEC_PARAM_BOND                  0                                                    /**< Perform bonding. */
#define SEC_PARAM_MITM                  0                                                    /**< Man In The Middle protection not required. */
#define SEC_PARAM_IO_CAPABILITIES       BLE_GAP_IO_CAPS_NONE                                 /**< No I/O capabilities. */
#define SEC_PARAM_OOB                   0                                                    /**< Out Of Band data not available. */
#define SEC_PARAM_MIN_KEY_SIZE          7                                                    /**< Minimum encryption key size. */
#define SEC_PARAM_MAX_KEY_SIZE          16                                                   /**< Maximum encryption key size. */

#define BLE_UUID_APPLE_NOTIFICATION_CENTER_SERVICE  0xf431                               /*<< ANCS service UUID. */
#define BLE_UUID_ANCS_CONTROL_POINT_CHAR            0xd8f3                               /*<< Control point UUID. */
#define BLE_UUID_ANCS_NOTIFICATION_SOURCE_CHAR      0x120d                               /*<< Notification source UUID. */
#define BLE_UUID_ANCS_DATA_SOURCE_CHAR              0xc6e9                               /*<< Data source UUID. */

#define BLE_CCCD_NOTIFY_BIT_MASK         0x0001                                            /**< Enable Notification bit. */


typedef enum
{
    BLE_NO_ADVERTISING,                                                                      /**< No advertising running. */
    BLE_SLOW_ADVERTISING,                                                                    /**< Slow advertising running. */
    BLE_FAST_ADVERTISING                                                                     /**< Fast advertising running. */
} ble_advertising_mode_t;

typedef enum
{
    STATE_UNINITIALIZED,                    // Program start.
    STATE_ADVERTISING,                      // Advertising. See Settings>Bluetooth on iPhone then connect to "ANCC"
    STATE_CONNECTED,                        // iPhone connected to us.
    STATE_DISCOVERY_SERVICE,                // Searching for ANCS Service.
    STATE_DISCOVERY_CHARACTERISTICS,        // Searching for ANCS Characteristics.
    STATE_PAIRING,                          // Got all we need. Now pair before subscribe. Should see pairing dialog on iPhone
    STATE_SUBSCRIBING,                      // Subscribe to CCC, for notification.
    STATE_LISTENING,                        // Listening...
    STATE_NOTIFIED,                         // Got notification, now retrieve other info and print log out. back to listening.
    STATE_DISCONNECTED,                     // Disconnected?
    STATE_ERROR
} state_t;



DigitalOut led_adv(LED1);
DigitalOut led_conn(LED2);

Serial     pc(USBTX, USBRX);

const ble_uuid128_t ble_ancs_base_uuid128 =
{
   {
    // 7905F431-B5CE-4E99-A40F-4B1E122D00D0
    0xd0, 0x00, 0x2d, 0x12, 0x1e, 0x4b, 0x0f, 0xa4,
    0x99, 0x4e, 0xce, 0xb5, 0x31, 0xf4, 0x05, 0x79
   }
};


const ble_uuid128_t ble_ancs_cp_base_uuid128 =
{
   {
    // 69d1d8f3-45e1-49a8-9821-9bbdfdaad9d9
    0xd9, 0xd9, 0xaa, 0xfd, 0xbd, 0x9b, 0x21, 0x98,
    0xa8, 0x49, 0xe1, 0x45, 0xf3, 0xd8, 0xd1, 0x69

   }
};

const ble_uuid128_t ble_ancs_ns_base_uuid128 =
{
   {
    // 9FBF120D-6301-42D9-8C58-25E699A21DBD
    0xbd, 0x1d, 0xa2, 0x99, 0xe6, 0x25, 0x58, 0x8c,
    0xd9, 0x42, 0x01, 0x63, 0x0d, 0x12, 0xbf, 0x9f

   }
};

const ble_uuid128_t ble_ancs_ds_base_uuid128 =
{
   {
    // 22EAC6E9-24D6-4BB5-BE44-B36ACE7C7BFB
    0xfb, 0x7b, 0x7c, 0xce, 0x6a, 0xb3, 0x44, 0xbe,
    0xb5, 0x4b, 0xd6, 0x24, 0xe9, 0xc6, 0xea, 0x22

   }
};

static state_t                          m_state = STATE_UNINITIALIZED;

static ble_gap_adv_params_t             m_adv_params;                                        /**< Parameters to be passed to the stack when starting advertising. */
static ble_advertising_mode_t           m_advertising_mode;                                  /**< Variable to keep track of when we are advertising. */

static ble_gap_sec_params_t             m_sec_params;                                        /**< Security requirements for this application. */

// ANCS Characteristic...
static uint16_t m_notification_source_handle = 0;
static uint16_t m_notification_source_handle_cccd = 0;
static uint16_t m_control_point_handle = 0;
static uint16_t m_data_source_handle = 0;
static uint16_t m_data_source_handle_cccd = 0;

static void err_check(uint32_t error_code, char *method)
{
    if(error_code != NRF_SUCCESS) {
        pc.printf("ERROR: %d (%s) on %s\r\n", error_code, error2string(error_code), method);
//    } else {
//        pc.printf("SUCCESS: %s\r\n", method);
    }
    APP_ERROR_CHECK(error_code);
}


static void advertising_start(void)
{
    uint32_t err_code;

    if (m_advertising_mode == BLE_NO_ADVERTISING)
    {
        m_advertising_mode = BLE_FAST_ADVERTISING;
    }
    else
    {
        m_advertising_mode = BLE_SLOW_ADVERTISING;
    }

    memset(&m_adv_params, 0, sizeof(m_adv_params));
    
    m_adv_params.type        = BLE_GAP_ADV_TYPE_ADV_IND;
    m_adv_params.p_peer_addr = NULL;                           // Undirected advertisement.
    m_adv_params.fp          = BLE_GAP_ADV_FP_ANY;

    if (m_advertising_mode == BLE_FAST_ADVERTISING)
    {
        m_adv_params.interval = APP_ADV_INTERVAL;
        m_adv_params.timeout  = ADV_INTERVAL_FAST_PERIOD;
    }
    else
    {
        m_adv_params.interval = APP_ADV_INTERVAL_SLOW;
        m_adv_params.timeout  = APP_ADV_TIMEOUT_IN_SECONDS;
    }

    err_code = sd_ble_gap_adv_start(&m_adv_params);
    err_check(err_code, "sd_ble_gap_adv_start");

    led_adv = 1;
    m_state = STATE_ADVERTISING;
}




static void ble_event_handler(ble_evt_t * p_ble_evt)
{
    uint32_t        err_code = NRF_SUCCESS;
    static uint16_t m_conn_handle = BLE_CONN_HANDLE_INVALID;
    ble_uuid_t ancs_uuid;
    static ble_gattc_handle_range_t handle_range;

    pc.printf("Event: %s\r\n", event2string(p_ble_evt));
//    ble_bondmngr_on_ble_evt(p_ble_evt);
//    ble_conn_params_on_ble_evt(p_ble_evt);
    
    switch (p_ble_evt->header.evt_id)
    {
        case BLE_GAP_EVT_CONNECTED:
        {
            m_state = STATE_CONNECTED;

            m_advertising_mode = BLE_NO_ADVERTISING;
            m_conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
            led_conn = 1;
            
            m_state = STATE_DISCOVERY_SERVICE;

            BLE_UUID_BLE_ASSIGN(ancs_uuid, BLE_UUID_APPLE_NOTIFICATION_CENTER_SERVICE);
            ancs_uuid.type = BLE_UUID_TYPE_VENDOR_BEGIN;

            err_code = sd_ble_gattc_primary_services_discover(m_conn_handle, 0x0001, &ancs_uuid);
            err_check(err_code, "sd_ble_gattc_primary_services_discover");            
            

            break;
        }        
        case BLE_GAP_EVT_AUTH_STATUS:
        {                
            m_state = STATE_SUBSCRIBING;

            // Subscribe to NS
            uint16_t       cccd_val = true ? BLE_CCCD_NOTIFY_BIT_MASK : 0;
            static ble_gattc_write_params_t m_write_params;
            uint8_t gattc_value[2];
                     
            gattc_value[0] = LSB(cccd_val);
            gattc_value[1] = MSB(cccd_val);

            m_write_params.handle = m_notification_source_handle_cccd;
            m_write_params.len  = 2;
            m_write_params.p_value = &gattc_value[0];
            m_write_params.offset = 0;
            m_write_params.write_op = BLE_GATT_OP_WRITE_REQ;
                        

            err_code = sd_ble_gattc_write(m_conn_handle, &m_write_params);
            err_check(err_code, "sd_ble_gattc_write");

            break;
        }   
        case BLE_GAP_EVT_DISCONNECTED:
        {
            m_conn_handle = BLE_CONN_HANDLE_INVALID;

            advertising_start();
            led_conn = 0;
            break;
        }    
        case BLE_GAP_EVT_SEC_PARAMS_REQUEST:
        {
            err_code = sd_ble_gap_sec_params_reply(m_conn_handle, 
                                                   BLE_GAP_SEC_STATUS_SUCCESS, 
                                                   &m_sec_params);
            err_check(err_code, "sd_ble_gap_sec_params_reply");
            break;
        }
        case BLE_GAP_EVT_TIMEOUT:
        {
            if (p_ble_evt->evt.gap_evt.params.timeout.src == BLE_GAP_TIMEOUT_SRC_ADVERTISEMENT)
            { 
                if (m_advertising_mode == BLE_FAST_ADVERTISING)
                {
                    advertising_start();
                }
                else
                {
                    err_code = sd_power_system_off();
                }
            }
            break;
        }   
        case BLE_GATTC_EVT_PRIM_SRVC_DISC_RSP:
        {
             if (p_ble_evt->evt.gattc_evt.gatt_status != BLE_GATT_STATUS_SUCCESS) {
                // Error.
                pc.printf("Error: %s\r\n", "BLE_GATTC_EVT_PRIM_SRVC_DISC_RSP");
            } else {
                if (p_ble_evt->evt.gattc_evt.params.prim_srvc_disc_rsp.count > 0) {
                    const ble_gattc_service_t * p_service;

                    p_service = &(p_ble_evt->evt.gattc_evt.params.prim_srvc_disc_rsp.services[0]);

                    pc.printf("Found ANCS service, start handle: %d, end handle: %d\r\n", 
                        p_service->handle_range.start_handle, p_service->handle_range.end_handle);

                    handle_range.start_handle = p_service->handle_range.start_handle;
                    handle_range.end_handle   = p_service->handle_range.end_handle;

                    err_code = sd_ble_gattc_characteristics_discover(m_conn_handle, &handle_range);
                    err_check(err_code, "sd_ble_gattc_characteristics_discover");
                    
                    m_state = STATE_DISCOVERY_CHARACTERISTICS;

                } else {
                    pc.printf("Error: discovery failure, no ANCS\r\n");
                }
            }

            break;
        }
        case BLE_GATTC_EVT_CHAR_DISC_RSP:
        {    
            // End of characteristics searching...no more attribute or no more handle.
            // We got error as response, but this is normal for gatt attribute searching.
            if (p_ble_evt->evt.gattc_evt.gatt_status == BLE_GATT_STATUS_ATTERR_ATTRIBUTE_NOT_FOUND ||
                p_ble_evt->evt.gattc_evt.gatt_status == BLE_GATT_STATUS_ATTERR_INVALID_HANDLE) {
                
                if(m_notification_source_handle == 0) {
                    pc.printf("Error: NS not found.\r\n");
                } else if(m_control_point_handle == 0) {
                    pc.printf("Error: CP not found.\r\n");
                } else if(m_data_source_handle == 0) {
                    pc.printf("Error: DS not found.\r\n");
                } 
                // OK, we got all char handles. Next, find CCC.
                else {
                    // Start with NS CCC
                    handle_range.start_handle = m_notification_source_handle + 1;
                    handle_range.end_handle = m_notification_source_handle + 1;
                    
                    err_code = sd_ble_gattc_descriptors_discover(m_conn_handle, &handle_range);
                    err_check(err_code, "sd_ble_gattc_descriptors_discover");
                }        
            
            } else if (p_ble_evt->evt.gattc_evt.gatt_status != BLE_GATT_STATUS_SUCCESS) {
                pc.printf("Error: %s\r\n", "BLE_GATTC_EVT_CHAR_DISC_RSP");
            } else {
                uint32_t                 i;
                const ble_gattc_char_t * p_char_resp = NULL;

                // Iterate trough the characteristics and find the correct one.
                 for (i = 0; i < p_ble_evt->evt.gattc_evt.params.char_disc_rsp.count; i++) {
                    p_char_resp = &(p_ble_evt->evt.gattc_evt.params.char_disc_rsp.chars[i]);
                    switch (p_char_resp->uuid.uuid) {
                        case BLE_UUID_ANCS_CONTROL_POINT_CHAR:
                            pc.printf("Found char: Control Point");
                            m_control_point_handle = p_char_resp->handle_value;
                        break;

                        case BLE_UUID_ANCS_NOTIFICATION_SOURCE_CHAR:
                            pc.printf("Found char: Notification Source");
                            m_notification_source_handle = p_char_resp->handle_value;
                        break;

                        case BLE_UUID_ANCS_DATA_SOURCE_CHAR:
                            pc.printf("Found char: Data Source");
                            m_data_source_handle = p_char_resp->handle_value;
                        break;

                        default:
                        break;
                    }
                }
                
                if(p_char_resp!=NULL) {

                    handle_range.start_handle = p_char_resp->handle_value + 1;

                    err_code = sd_ble_gattc_characteristics_discover(m_conn_handle, &handle_range);
                    err_check(err_code, "sd_ble_gattc_characteristics_discover");
                    
                } else {
                    err_code = sd_ble_gattc_characteristics_discover(m_conn_handle, &handle_range);
                    err_check(err_code, "sd_ble_gattc_characteristics_discover");
                }
        
            }
        
            break;
        }
        case BLE_GATTC_EVT_DESC_DISC_RSP:
        {
            if (p_ble_evt->evt.gattc_evt.gatt_status != BLE_GATT_STATUS_SUCCESS) {
                pc.printf("Error: %s\r\n", "BLE_GATTC_EVT_DESC_DISC_RSP");        
            } else {
                if (p_ble_evt->evt.gattc_evt.params.desc_disc_rsp.count > 0) {
                    const ble_gattc_desc_t * p_desc_resp = &(p_ble_evt->evt.gattc_evt.params.desc_disc_rsp.descs[0]);
                    if (p_desc_resp->uuid.uuid == BLE_UUID_DESCRIPTOR_CLIENT_CHAR_CONFIG) {
                        if(p_desc_resp->handle == m_notification_source_handle + 1) {
                                                                          
                            m_notification_source_handle_cccd = p_desc_resp->handle;
                            pc.printf("Found NS CCC\r\n");
                            
                            // Next, find CCC for data source.
                            handle_range.start_handle = m_data_source_handle + 1;
                            handle_range.end_handle = m_data_source_handle + 1;
                    
                            err_code = sd_ble_gattc_descriptors_discover(m_conn_handle, &handle_range);
                            err_check(err_code, "sd_ble_gattc_descriptors_discover");

                        } else if(p_desc_resp->handle == m_data_source_handle + 1) {
                            
                            m_data_source_handle_cccd = p_desc_resp->handle;
                            pc.printf("Found DS CCC\r\n");
                                                                  
                            // Got all we need, now before subscribing we'll do pairing.
                            // request encryption...., we are in peripheral role.
                            
                            m_state = STATE_PAIRING;
                            
                            err_code = sd_ble_gap_authenticate(m_conn_handle, &m_sec_params);
                            err_check(err_code, "sd_ble_gap_authenticate");
                        }
                    }
                }
            }
            break;
        }
        case BLE_GATTC_EVT_WRITE_RSP:
        {
            if (p_ble_evt->evt.gattc_evt.gatt_status != BLE_GATT_STATUS_SUCCESS) {
                pc.printf("Error: %s\r\n", "BLE_GATTC_EVT_WRITE_RSP"); 

                if(p_ble_evt->evt.gattc_evt.params.write_rsp.handle == m_control_point_handle) {
                    m_state = STATE_LISTENING;
                }
            } else {
                if(p_ble_evt->evt.gattc_evt.params.write_rsp.handle == m_notification_source_handle_cccd) {
                    pc.printf("NS subscribe success.\r\n");

                    // Next, subscribe to DS.
                    uint16_t       cccd_val = true ? BLE_CCCD_NOTIFY_BIT_MASK : 0;
                    static ble_gattc_write_params_t m_write_params;
                    uint8_t gattc_value[2];
                     
                    gattc_value[0] = LSB(cccd_val);
                    gattc_value[1] = MSB(cccd_val);

                    m_write_params.handle = m_data_source_handle_cccd;
                    m_write_params.len  = 2;
                    m_write_params.p_value = &gattc_value[0];
                    m_write_params.offset = 0;
                    m_write_params.write_op = BLE_GATT_OP_WRITE_REQ;
                        
                    err_code = sd_ble_gattc_write(m_conn_handle, &m_write_params);
                    err_check(err_code, "sd_ble_gattc_write");
                }
                
                if(p_ble_evt->evt.gattc_evt.params.write_rsp.handle == m_data_source_handle_cccd) {
                    pc.printf("DS subscribe success.\r\n");
                    
                    // Now, we just waiting for NS notification.
                    m_state = STATE_LISTENING;
                }
                
                if(p_ble_evt->evt.gattc_evt.params.write_rsp.handle == m_control_point_handle) {
                    pc.printf("CP write success.\r\n");
                    // We'll receive data from DS notification
                }

            }
        
            break;
        }
        case BLE_GATTC_EVT_HVX:
        {
            if (p_ble_evt->evt.gattc_evt.gatt_status != BLE_GATT_STATUS_SUCCESS) {
                pc.printf("Error: %s\r\n", "BLE_GATTC_EVT_HVX");        
            } else {

                // Got notification...
                if(p_ble_evt->evt.gattc_evt.params.hvx.handle == m_notification_source_handle) {
                    ble_gattc_evt_hvx_t *p_hvx = &p_ble_evt->evt.gattc_evt.params.hvx;
                    if(p_hvx->len == 8) {
                        pc.printf("Event ID: %x (%s)\r\n", p_hvx->data[0], eventid2string(p_hvx->data[0]));
                        pc.printf("Event Flags: %x (%s)\r\n", p_hvx->data[1], eventflags2string(p_hvx->data[1]));
                        pc.printf("Category ID: %x (%s)\r\n", p_hvx->data[2], categoryid2string(p_hvx->data[2]));
                        pc.printf("Category Count: %x\r\n", p_hvx->data[3]);
                        pc.printf("Notification ID: %x %x %x %x\r\n", p_hvx->data[4], p_hvx->data[5], p_hvx->data[6], p_hvx->data[7]);
                        
                        // if we are still processing, we can not do another write
                        // with soft device (limitation?). Real implementation should use
                        // queue to synchronized operation. Since this is a POC... just ignore.
                        if(m_state == STATE_NOTIFIED) {
                            pc.printf("Still retrieving data for another notification. ignoring this one.\r\n");
                        } else if(p_hvx->data[0] == 0) {
                            // we only retrieved data for added notification.
                            m_state = STATE_NOTIFIED;
                            // write control point to get another data.
                            
                            // We only retrieve the title, with 16 bytes buffer... see ANCS spec for more
                            static ble_gattc_write_params_t m_write_params;
                            uint8_t gattc_value[8];
                     
                            gattc_value[0] = 0; // CommandIDGetNotificationAttributes
                            gattc_value[1] = p_hvx->data[4];
                            gattc_value[2] = p_hvx->data[5];
                            gattc_value[3] = p_hvx->data[6];
                            gattc_value[4] = p_hvx->data[7];
                            gattc_value[5] = 1; // Title
                            gattc_value[6] = 16; // Length, 2 bytes, MSB first.
                            gattc_value[7] = 0;

                            m_write_params.handle = m_control_point_handle;
                            m_write_params.len  = 8;
                            m_write_params.p_value = &gattc_value[0];
                            m_write_params.offset = 0;
                            m_write_params.write_op = BLE_GATT_OP_WRITE_REQ;
                        
                            err_code = sd_ble_gattc_write(m_conn_handle, &m_write_params);
                            err_check(err_code, "sd_ble_gattc_write");
                            
                        }
                        
                    } else {
                        pc.printf("NS data len not 8\r\n");
                    }
                }
                
                // Got data
                if(p_ble_evt->evt.gattc_evt.params.hvx.handle == m_data_source_handle) {
                    ble_gattc_evt_hvx_t *p_hvx = &p_ble_evt->evt.gattc_evt.params.hvx;
                    pc.printf("Title:");
                    // we only set size on MSB...
                    uint16_t len = p_hvx->data[6];
                    pc.printf("(%d)", len);

                    // the data itself start from index 8 to 8+len;
                    uint16_t pos;
                    for(pos=8; pos<=8+len; pos++) {
                        pc.printf("%c", p_hvx->data[pos]);
                    }
                    pc.printf("\r\n");
                    
                    // Back to listening...
                    m_state = STATE_LISTENING;
                }
                
            }
            break;
        }
        case BLE_GATTC_EVT_TIMEOUT:
        case BLE_GATTS_EVT_TIMEOUT:
        {
            // Disconnect on GATT Server and Client timeout events.
            err_code = sd_ble_gap_disconnect(m_conn_handle, 
                                             BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
            err_check(err_code, "sd_ble_gap_disconnect");
            break;
        }
        default:
        {
            //No implementation needed
            break;
        }
    }

}


static void sys_event_handler(uint32_t sys_evt)
{
    pc.printf("Event: system event\r\n");
    pstorage_sys_event_handler(sys_evt);
}


static void timers_init(void)
{
    // Initialize timer module.
    APP_TIMER_INIT(APP_TIMER_PRESCALER, APP_TIMER_MAX_TIMERS, APP_TIMER_OP_QUEUE_SIZE, false);
}


static void ble_stack_init(void)
{
    uint32_t err_code;
    
    // Initialize the SoftDevice handler module.
//    SOFTDEVICE_HANDLER_INIT(NRF_CLOCK_LFCLKSRC_XTAL_20_PPM, false);
    SOFTDEVICE_HANDLER_INIT(NRF_CLOCK_LFCLKSRC_RC_250_PPM_4000MS_CALIBRATION, false);
    

    // Register with the SoftDevice handler module for BLE events.
    err_code = softdevice_ble_evt_handler_set(ble_event_handler);
    err_check(err_code, "softdevice_ble_evt_handler_set");
    
    // Register with the SoftDevice handler module for System events.
    err_code = softdevice_sys_evt_handler_set(sys_event_handler);
    err_check(err_code, "softdevice_sys_evt_handler_set");
}

static void set_128_uuid()
{
    uint32_t        err_code;
    uint8_t         temp_type;  // All ANCS is vendor type... so we ignore this.


    err_code = sd_ble_uuid_vs_add(&ble_ancs_base_uuid128, &temp_type);
    err_check(err_code, "sd_ble_uuid_vs_add");

    err_code = sd_ble_uuid_vs_add(&ble_ancs_cp_base_uuid128, &temp_type);
    err_check(err_code, "sd_ble_uuid_vs_add");

    err_code = sd_ble_uuid_vs_add(&ble_ancs_ns_base_uuid128, &temp_type);
    err_check(err_code, "sd_ble_uuid_vs_add");

    err_code = sd_ble_uuid_vs_add(&ble_ancs_ds_base_uuid128, &temp_type);
    err_check(err_code, "sd_ble_uuid_vs_add");
}


static void conn_params_error_handler(uint32_t nrf_error)
{
    err_check(nrf_error, "Error: conn params error");
}


static void conn_params_init(void)
{
    uint32_t               err_code;
    ble_conn_params_init_t cp_init;

    memset(&cp_init, 0, sizeof(cp_init));

    cp_init.p_conn_params                  = NULL;
    cp_init.first_conn_params_update_delay = FIRST_CONN_PARAMS_UPDATE_DELAY;
    cp_init.next_conn_params_update_delay  = NEXT_CONN_PARAMS_UPDATE_DELAY;
    cp_init.max_conn_params_update_count   = MAX_CONN_PARAMS_UPDATE_COUNT;
    cp_init.start_on_notify_cccd_handle    = BLE_GATT_HANDLE_INVALID;
    cp_init.disconnect_on_fail             = true;
    cp_init.evt_handler                    = NULL;
    cp_init.error_handler                  = conn_params_error_handler;

    err_code = ble_conn_params_init(&cp_init);
    err_check(err_code, "ble_conn_params_init");
}


static void sec_params_init(void)
{
    m_sec_params.timeout      = SEC_PARAM_TIMEOUT;
    m_sec_params.bond         = SEC_PARAM_BOND;
    m_sec_params.mitm         = SEC_PARAM_MITM;
    m_sec_params.io_caps      = SEC_PARAM_IO_CAPABILITIES;
    m_sec_params.oob          = SEC_PARAM_OOB;  
    m_sec_params.min_key_size = SEC_PARAM_MIN_KEY_SIZE;
    m_sec_params.max_key_size = SEC_PARAM_MAX_KEY_SIZE;
}


static void gap_params_init(void)
{
    uint32_t                err_code;
    ble_gap_conn_params_t   gap_conn_params;
    ble_gap_conn_sec_mode_t sec_mode;

    BLE_GAP_CONN_SEC_MODE_SET_OPEN(&sec_mode);
    
    err_code = sd_ble_gap_device_name_set(&sec_mode, 
                                          (const uint8_t *)DEVICE_NAME, 
                                          strlen(DEVICE_NAME));
    err_check(err_code, "sd_ble_gap_device_name_set");

    memset(&gap_conn_params, 0, sizeof(gap_conn_params));

    gap_conn_params.min_conn_interval = MIN_CONN_INTERVAL;
    gap_conn_params.max_conn_interval = MAX_CONN_INTERVAL;
    gap_conn_params.slave_latency     = SLAVE_LATENCY;
    gap_conn_params.conn_sup_timeout  = CONN_SUP_TIMEOUT;

    err_code = sd_ble_gap_ppcp_set(&gap_conn_params);
    err_check(err_code, "sd_ble_gap_ppcp_set");
}


static void advertising_init(void)
{
    uint32_t      err_code;
    ble_advdata_t advdata;
    uint8_t       flags = BLE_GAP_ADV_FLAGS_LE_ONLY_LIMITED_DISC_MODE;
    ble_uuid_t    ancs_uuid;
    
//    err_code = sd_ble_uuid_vs_add(&ble_ancs_base_uuid128, &m_ancs_uuid_type);
//    err_check(err_code, "sd_ble_uuid_vs_add");

//    ancs_uuid.uuid = ((ble_ancs_base_uuid128.uuid128[12]) | (ble_ancs_base_uuid128.uuid128[13] << 8));
//    ancs_uuid.type = m_ancs_uuid_type;
    BLE_UUID_BLE_ASSIGN(ancs_uuid, BLE_UUID_APPLE_NOTIFICATION_CENTER_SERVICE);
    ancs_uuid.type = BLE_UUID_TYPE_VENDOR_BEGIN;

    // Build and set advertising data.
    memset(&advdata, 0, sizeof(advdata));
    
    advdata.name_type               = BLE_ADVDATA_FULL_NAME;
    advdata.include_appearance      = true;
    advdata.flags.size              = sizeof(flags);
    advdata.flags.p_data            = &flags;
    advdata.uuids_complete.uuid_cnt = 0;
    advdata.uuids_complete.p_uuids  = NULL;
    advdata.uuids_solicited.uuid_cnt = 1;
    advdata.uuids_solicited.p_uuids  = &ancs_uuid;    
    
    err_code = ble_advdata_set(&advdata, NULL);
    err_check(err_code, "ble_advdata_set");

}


/**************************************************************************/
/*!
    @brief  Program entry point
*/
/**************************************************************************/
int main(void)
{
    uint32_t err_code;
//    uint32_t soc_event;
//    uint32_t evt_id;
    
    pc.printf("Program started\n\r");
        
    led_adv = 0;
    led_conn = 0;


    pc.printf("timers_init()\r\n");
    timers_init();
    
    pc.printf("ble_stack_init()\r\n");
    ble_stack_init();    

    /* Make sure we get a clean start */
    wait(0.5);
    wait(1);

    pc.printf("gap_params_init()\r\n");
    gap_params_init();

    pc.printf("set_128_uuid()\r\n");
    set_128_uuid();
    
    pc.printf("advertising_init()\r\n");
    advertising_init();
    

    pc.printf("conn_params_init()\r\n");
    conn_params_init();
    
    pc.printf("sec_params_init()\r\n");
    sec_params_init();
        
    pc.printf("advertising_start()\r\n");
    advertising_start();


//    while(1) { wait(1.0); };

    for (;;)
    {
        err_code = sd_app_evt_wait();
        err_check(err_code, "sd_app_evt_wait");
        
        /*
        do {
            soc_event = sd_evt_get(&evt_id);
            pc.printf("soc_event: %d\r\n", evt_id);
        } while(soc_event != NRF_ERROR_NOT_FOUND);
        */
    }
    
}