File content as of revision 3:3f35e80ed848:

#include <BleSlaveService.h>


/* ----- Private variables ----- */
/* Struct of slave dev */
SlaveDevice_t slaveDev;


ChangeNotificationQueue notifyQ, *notifyP;


/* Struct of periph dev */
extern PeripheralDevices_t perDevs;

/* Notification frequency */
uint8_t notification_freq = NOTIFICATION_FREQ_WIFI_OFF;


/* Star Service and Characteristic */
uint8_t sUuid[16]    = {0x00,0x00,0x00,0x00,0x00,0x01,0x11,0xe1,0x9a,0xb4,0x00,0x02,0xa5,0xd5,0xc5,0x1b};
uint8_t dataUuid[16] = {0x00,0x08,0x00,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b};
uint8_t confUuid[16] = {0x00,0x04,0x00,0x00,0x00,0x01,0x11,0xe1,0xac,0x36,0x00,0x02,0xa5,0xd5,0xc5,0x1b};


/*---- Create the two characteristic ----*/
/* Config char */
GattCharacteristic configChar(confUuid, slaveDev.star_config_value, STAR_CHAR_MAX_VALUE_LEN, STAR_CHAR_MAX_VALUE_LEN,
               GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY|GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ|GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE,
               NULL, 0, true);


/* Data char */
GattCharacteristic dataChar(dataUuid, slaveDev.notification_data.attribute_value, STAR_CHAR_MAX_VALUE_LEN, STAR_CHAR_MAX_VALUE_LEN,
             GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY|GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ|GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE,
             NULL, 0, true);


/* MANUFACTURER_SPECIFIC_DATA */
    uint8_t manuf_data[6] = {
                0x01, /*SKD version */
                0x81, /* NUCLEO-Board remote */
                0x00, /* Led Prox+Lux */ //Unused
                0x0C, /* Star Config Char + Star Data Char */ //Unused
                0x00, /* SensorFusionShort */ //Unused
                0x00 /* SensorFusionFloat */ //Unused
    };





/*----------------------------------------------------------------------------*/


/* Set the device as a slave in discoverable mode */
void setSlaveDiscoverable(void) {
    //printf("\r\nsetSlaveDiscoverable\n");//DEBUG
    BLE& ble = BLE::Instance();
    ble_error_t e2;


    //if-MAIN - discovery/reading/writing ops completed
    if ((discoveryCompleted == 1) /*&& (readCompleted == 1) && (writeDescriptorCompleted == 1)*/){


        /* !! Solve advertising after master disconnection */
        
        /* Start advertisng */
        if ((!ble.gap().getState().advertising)){
            advEnds =0;

            printf("\r\nSet Discoverable Mode (%d)\n", perDevs.status);
            e2 = ble.gap().startAdvertising();
            if ( e2 == BLE_ERROR_NONE ){
                /* Giving some time to connect */
                eventQ.call_in(7000, stopAdv);//N ms
            }else{
                printf("\r\nError while starting advertising (err: %d - stat: %d)\n", e2, perDevs.status);
                advEnds=1;//terminated because not started
            }

        }//if-not-adv
    }//if-MAIN


    //Restart loop ops
    perDevs.status = CONN_INIT;
    perDevs.discovery_enabled = true;
    //printf("\rstatus = CONN_INIT (%d)\n", perDevs.status);//DEBUG

}
/*----------------------------------------------------------------------------*/



void stopAdv(){

    if ((slaveDev.is_connected == false) && (discoveryCompleted == 1)){
        if (BLE::Instance().gap().getState().advertising){
            ble_error_t e = BLE::Instance().gap().stopAdvertising();
            if ( e != BLE_ERROR_NONE ) {
                printf("\r\nError stopping ADV\n");//DEBUG
            }
        }
    }//if

    advEnds = 1;
}
/*----------------------------------------------------------------------------*/



/**
 * @brief  This function sets some notification properties (e.g. the
 *         notification period) by sending a command according to the
 *         following format:
 *         FEATURE_MASK (4bytes) + Command (1byte) + Data (1byte)
 *         In case of the notification period the command is 255 while the allowed data are:
 *         50 @5S, 10 @1S, 1 @100mS, 0 @50mS (default).
 * @param  connection handle
 * @param  peripheral device index
 * @param  feature mask
 * @param  command
 * @param  data
 * @retval None
 */
void setNotificationProperty(uint16_t conn_handle, uint8_t i, uint32_t feature_mask,
                             uint8_t command, uint8_t data) {
    //printf("\r\nsetNotificationProperty\n");//DEBUG

    uint16_t attr_handle;
    uint8_t  value_len;
    uint8_t  attr_value[6];

    attr_handle = perDevs.cfg_char_handle[i] + 1;
    attr_value[0] = (uint8_t)((feature_mask) >> 24);
    attr_value[1] = (uint8_t)((feature_mask) >> 16);
    attr_value[2] = (uint8_t)((feature_mask) >> 8);
    attr_value[3] = (uint8_t)((feature_mask));
    attr_value[4] = command;
    attr_value[5] = data;
    value_len = 6;  /* FEATURE_MASK len (4bytes) + Command len (1byte) + Data len (1byte) */
        
    /* The gatt write function above is w/out response, so we just wait for a short time before going on */
    writeCharacValueWithoutResp(conn_handle, attr_handle, value_len, attr_value);
    HAL_Delay(2000);//N ms
}
/*----------------------------------------------------------------------------*/




/* Forwards to master all notifications from peripherals */
void notifyMaster(uint8_t data_length, uint8_t* attribute_value, uint16_t attribute_handle){
    //printf("\r\nnotifyMaster\n");//DEBUG
    ble_error_t notifyError;

        
        notifyError = BLE::Instance().gattServer().write(attribute_handle+1, attribute_value, data_length, false);

        if (notifyError != BLE_ERROR_NONE){
            printf("\r\n(notifyMaster) Error (%d) while updating characteristic\n", notifyError);//BLE_STACK_BUSY
            if (notifyError == BLE_STACK_BUSY){
                stackBusy = 1;
                return;
            }
        }else {
                stackBusy = 0;
                return;
        }
    
}
/*----------------------------------------------------------------------------*/


/* Add all services using a vendor specific UUIDs */
void addAllServices(void){

    ble_error_t error;
    notifyP = &notifyQ;


    GattCharacteristic *charTable[] = {&configChar, &dataChar};

    /*---- Create the hw service ----*/
    GattService starService(sUuid, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));

    /*---- Add the hw service ----*/
    error = BLE::Instance().gattServer().addService(starService);
    slaveDev.star_hw_serv_handle     = starService.getHandle();
    slaveDev.star_config_char_handle = configChar.getValueHandle()-1;   //-1 for decl handle
    slaveDev.star_data_char_handle   = dataChar.getValueHandle()-1;     //-1 for decl handle



    if ( error != BLE_ERROR_NONE ){
        printf("\r\nError while creating starService(error: %d)\n", error);

    } else {
        printf("\r\nStar HW Gatt Service added (handle 0x%004x)\n", slaveDev.star_hw_serv_handle);
        printf("\rStar HW Config Char added (handle 0x%004x)\n", slaveDev.star_config_char_handle);
        printf("\rStar HW Data Char added (handle 0x%004x)", slaveDev.star_data_char_handle);
        printf("\r\n");
    }

}
/*----------------------------------------------------------------------------*/


/* Disable all notifications */
void disableAllNotifications(void){
    //printf("\r\ndisableAllNotifications\n");//DEBUG

    
    uint8_t j;
    uint16_t attr_handle;
    uint8_t  value_len;
    uint8_t  attr_value[2];
    char* feat = NULL;

    for (j=0; j<(perDevs.connDevices+perDevs.discDevices); j++) {

        if ((perDevs.mic_event[j]) && (perDevs.mic_event_enabled))  {
            feat = "MIC";
            perDevs.mic_event_enabled = 0;
            perDevs.mic_event[j] = 0;
            attr_handle = perDevs.mic_char_handle[j] + 2;

        } else if ((perDevs.prx_event[j]) && (perDevs.prx_event_enabled)) {
            feat = "PRX";
            perDevs.prx_event_enabled = 0;
            perDevs.prx_on[j] = 0;
            attr_handle = perDevs.prx_char_handle[j] + 2;

        } else if ((perDevs.agm_event[j]) && (perDevs.agm_event_enabled)) {
            feat = "AGM";
            perDevs.acc_event_enabled = 0;
            perDevs.gyr_event_enabled = 0;
            perDevs.mag_event_enabled = 0;
            perDevs.agm_event_enabled = 0;
            perDevs.agm_on[j] = 0;
            attr_handle = perDevs.agm_char_handle[j] + 2;

        } else if ((perDevs.sfusion_event[j]) && (perDevs.sfusion_event_enabled)) {
            feat = "SFUSION";
            perDevs.sfusion_event_enabled = 0;
            perDevs.sfusion_on[j] = 0;
            attr_handle = perDevs.sfusion_char_handle[j] + 2;

        } else {
            continue;
        }

        value_len = 2;
        attr_value[0] = DISABLE;
        

        writeDescriptorCompleted=0;
        ble_error_t disableErr = writeCharacDescriptorWithError(perDevs.connection_handle[j], attr_handle, value_len, attr_value);
        printf("\r\nSet OFF the %s notifications on node %d\n", feat, j);

        if (disableErr != BLE_ERROR_NONE){
            printf("\r\n(disableAllNotifications) Write charac descriptor failed (err: %d, stat: %d)\n", disableErr, perDevs.status);
            writeDescriptorCompleted=1;
        }//if-error
    }//for
}
/*----------------------------------------------------------------------------*/



/* This function enables or disables the new peripheral scanning */
void setNewNodesScanning(uint8_t enabled){

    STORE_LE_16(slaveDev.star_config_value,(HAL_GetTick()>>3)); /* Timestamp */

    switch (enabled) {
    case 0x01:
        perDevs.discovery_enabled = true;
        //printf("\r\nScanning enabled (%d)\n", enabled);
    break;

    case 0x02:
        perDevs.discovery_enabled = false;
        //printf("\r\nScanning disabled (%d)\n", enabled);
    break;

    default:
        //printf("\r\nScanning set to unrecognized value (%d)\n", enabled);
    break;
    }

    slaveDev.star_config_value[2] = enabled;
    slaveDev.star_config_value_len = 3;
    //HAL_Delay(300);

    /* Notify master for scanning */

/* Check the scan enable/disable */
//    notifyMaster(slaveDev.star_config_value_len, slaveDev.star_config_value,
//                 slaveDev.star_config_char_handle);

}
/*----------------------------------------------------------------------------*/


//NOT USED FOR NOW !!
/* This function forwards to the peripheral device the command coming from the Cloud */
void Forward_Command_To_BlueNRG(uint8_t* data_ptr, uint8_t data_length){

    uint16_t handle;
    uint8_t  buffer[4] = {0, 0, 0, 0};
    uint8_t  num, even, odd;
    uint8_t  i;
    uint8_t cmd_type;

    cmd_type = (data_ptr[0] - '0') << 4 | (data_ptr[1] - '0');
    //printf("\r\ncommand %d\n", cmd_type);

  switch (cmd_type)
  {
  case 0:
    printf("\rScan cmd type (%d)\n", cmd_type);
    handle = slaveDev.star_config_char_handle + 1;
    break;
  case 1:
    printf("\rLED/MIC/PRX cmd type (%d)\n", cmd_type);
    handle = slaveDev.star_data_char_handle + 1;
    break;
  default:
    printf("\rUnknown cmd type (%d)\n", cmd_type);
    return;
    //break;
  }

  /**
  * For Scan cmd type:
  * |Value  |
  * |1 byte |
  * For LED/MIC/PRX cmd type:
  * |Node ID | Type ID | Value   |
  * |2 bytes | 1 byte  | x bytes |
  */
  for (i=2; i<data_length; i++) {
    switch (data_ptr[i])
    {
    case 'A':
    case 'a':
      num = 10;
      break;
    case 'B':
    case 'b':
      num = 11;
      break;
    case 'C':
    case 'c':
      num = 12;
      break;
    case 'D':
    case 'd':
      num = 13;
      break;
    case 'E':
    case 'e':
      num = 14;
      break;
    case 'F':
    case 'f':
      num = 15;
      break;
    default:
      num = data_ptr[i] - '0';
      break;
    }
    if (!(i%2)) {
      even = num << 4;
    }
    else {
      odd = num;
      buffer[(i-2)/2] = even | odd;
    }
  }

  createAttribute_Modified_CB_Prototype(handle, (data_length-2)/2, buffer);
}
/*----------------------------------------------------------------------------*/


/* Function used to call AttributeModified_CB outside the callback call */
void createAttribute_Modified_CB_Prototype(uint16_t handle, uint8_t data_length, uint8_t *att_data){

    GattWriteCallbackParams paramStruct, *pointerToParam;
    pointerToParam = &paramStruct;

    pointerToParam->handle = handle;
    pointerToParam->len = data_length;
    pointerToParam->data = att_data;

    AttributeModified_CB(pointerToParam);
}
/*----------------------------------------------------------------------------*/




/* This function is called when a local attribute gets modified */
void AttributeModified_CB(const GattWriteCallbackParams* param){
//    printf("\r\nAttributeModified_CB (%d)\n", perDevs.status);//DEBUG


    /* Assign prototype vars */
    uint16_t handle     = param->handle;
    //uint8_t data_length = param->len; //not used
    uint8_t *att_data   = (uint8_t *)param->data;


    uint8_t  i;
    uint16_t conn_handle;
    uint16_t attr_handle;
    uint8_t  value_len;
    uint8_t  attr_value[6];
    char* mems;

    if (handle == slaveDev.star_config_char_handle + 1) {

        perDevs.status = ALL_DATA_READ;
        if (att_data[0]==0x01) {
            /* Disabling all previously enabled notifications */
            disableAllNotifications();
        }//if
        setNewNodesScanning(att_data[0]);


    } else if (handle == slaveDev.star_config_char_handle + 2) {
        if (att_data[0]==0x01) {
            //printf("\r\nEnable scanning notification\n");
            STORE_LE_16(slaveDev.star_config_value,(HAL_GetTick()>>3)); /* Timestamp */
            slaveDev.star_config_value[2] = perDevs.discovery_enabled;
            slaveDev.star_config_value_len = 3;

            /* send the status of the scanning when notifications are enabled */
            notifyMaster(slaveDev.star_config_value_len, slaveDev.star_config_value,
                         slaveDev.star_config_char_handle);

            /* enable here the data notifications since sometimes the relative command gets lost (FIXME) */
            //printf("\r\nEnable data notification (0)\n");
            slaveDev.star_data_char_notify = 1;

        } else if (att_data[0]==0x00) {
            //printf("\r\nDisable scanning notification\n");
        }

    } else if (handle == slaveDev.star_data_char_handle + 2) {
        if (att_data[0]==0x01) {
            //printf("\r\nEnable data notification (1)\n");
            slaveDev.star_data_char_notify = 1;


        } else if (att_data[0]==0x00) {
            //printf("\r\nDisable data notification\n");
            slaveDev.star_data_char_notify = 0;

        }

    } else if (handle == slaveDev.star_data_char_handle + 1) {

    /*
     * Forward the command to the proper peripheral node.
     * It can be:
     * - the ON/OFF of a LED
     * - the notification enabling/disabling for a MIC Level or a Proximity Sensor
     * The coming data are received according to the following format:
     * |Node ID | Type ID | value   |
     * |2 bytes | 1 byte  | x bytes |
     */
        
        i = Get_Device_Index_From_Addr(att_data);
        if ((i < MAX_NUM_OF_NODES) && (!perDevs.is_disconnected[i]) && (perDevs.cfg_char_handle[i] != 0)) {
            conn_handle = perDevs.connection_handle[i];

            switch (att_data[2]) {


            //LED_TYPE_ID
            case LED_TYPE_ID:
                attr_handle = perDevs.cfg_char_handle[i] + 1;
                value_len   = 5; /* 4 bytes for the feature mask + 1 byte for the command (0=ON, 1=OFF) */
                attr_value[0] = (uint8_t)(((uint32_t)FEATURE_MASK_LED_EVENTS) >> 24);
                attr_value[1] = (uint8_t)(((uint32_t)FEATURE_MASK_LED_EVENTS) >> 16);
                attr_value[2] = (uint8_t)(((uint32_t)FEATURE_MASK_LED_EVENTS) >> 8);
                attr_value[3] = (uint8_t)(((uint32_t)FEATURE_MASK_LED_EVENTS));
                attr_value[4] = att_data[3];

                writeCharacValueWithoutResp(conn_handle, attr_handle, value_len, attr_value);

                perDevs.readDeviceIdx = i;
            break;



            //MICLEVEL_TYPE_ID
            case MICLEVEL_TYPE_ID:
            
                if (readCompleted == 0){
                    notificationPending = 1;
                
                    /* Fill the temp struct */
                    notifyP->att_data     = att_data;
                    notifyP->attr_value   = attr_value;
                    notifyP->conn_handle  = conn_handle;
                    notifyP->i            = i;
                    notifyP->feature_mask = FEATURE_MASK_MIC;
                    notifyP->frequency    = notification_freq;    
                
                    /* Then, call the Change_Notification_Status from the readCallback with these params */
                
                }else if (readCompleted == 1) {
                    //notificationPending = 0;
                    Change_Notification_Status(att_data, attr_value, conn_handle, i, FEATURE_MASK_MIC, notification_freq);
                }
                perDevs.prx_on[i] = attr_value[0];
                printf("\r\nMIC notifications [%d] %s (status %d, discovery %d)\n", i, (attr_value[0]==1 ? "ON" : "OFF"), perDevs.status, perDevs.discovery_enabled);    
            
#if SCAN_AFTER_NOTIFICATION
                /* Restart peripheral discovery and notification*/
                if (attr_value[0]!=1){
                    perDevs.discovery_enabled = 1;
                    setNewNodesScanning(0x01);
                    //HAL_Delay(300);
                }
#endif
            break;





            //PRX_TYPE_ID
            case PRX_TYPE_ID:
            
                if (readCompleted == 0){
                    notificationPending = 1;    
                
                    /* Fill the temp struct */
                    notifyP->att_data     = att_data;
                    notifyP->attr_value   = attr_value;
                    notifyP->conn_handle  = conn_handle;
                    notifyP->i            = i;
                    notifyP->feature_mask = FEATURE_MASK_PROX;
                    notifyP->frequency    = notification_freq*SENDING_INTERVAL_100MS_MULTIPLE;
                
                    /* Then, call the Change_Notification_Status from the readCallback with these params */
                
                }else if (readCompleted == 1) {
                    //notificationPending = 0;
                    Change_Notification_Status(att_data, attr_value, conn_handle, i, FEATURE_MASK_PROX, notification_freq*SENDING_INTERVAL_100MS_MULTIPLE);
                }    
                    perDevs.prx_on[i] = attr_value[0];
                    printf("\r\nPRX notifications [%d] %s (status %d)\n", i, (attr_value[0]==1 ? "ON" : "OFF"), perDevs.status);

#if SCAN_AFTER_NOTIFICATION
                /* Restart peripheral discovery and notification*/
                if (attr_value[0]!=1){
                    perDevs.discovery_enabled = 1;
                    setNewNodesScanning(0x01);
                    //HAL_Delay(300);
                }
#endif
            break;



            //ACC_TYPE_ID
            //GYR_TYPE_ID
            //MAG_TYPE_ID
            case ACC_TYPE_ID:
            case GYR_TYPE_ID:
            case MAG_TYPE_ID:
                
                if (readCompleted == 0){
                    notificationPending = 1;        
                
                    /* Fill the temp struct */
                    notifyP->att_data     = att_data;
                    notifyP->attr_value   = attr_value;
                    notifyP->conn_handle  = conn_handle;
                    notifyP->i            = i;
                    notifyP->feature_mask = FEATURE_MASK_ACC;
                    notifyP->frequency    = SENDING_INTERVAL_100MS_MULTIPLE;
                
                
                    /* Then, call the Change_Notification_Status from the readCallback with these params */
                
                }else if (readCompleted == 1) {
                    //notificationPending = 0;
                    Change_Notification_Status(att_data, attr_value, conn_handle, i, FEATURE_MASK_ACC, SENDING_INTERVAL_100MS_MULTIPLE);
                }
                perDevs.agm_on[i] = attr_value[0];

                if (att_data[2] == 0x09) {
                    perDevs.acc_event_enabled = attr_value[0];
                    mems = "ACC";

                } else if (att_data[2] == 0x0A) {
                    perDevs.gyr_event_enabled = attr_value[0];
                    mems = "GYR";

                } else if (att_data[2] == 0x0B) {
                    perDevs.mag_event_enabled = attr_value[0];
                    mems = "MAG";
                }

                printf("\r\nAGM notifications [%d] %s (%s) (status %d)\n", i, (attr_value[0]==1 ? "ON" : "OFF"), mems, perDevs.status);
#if SCAN_AFTER_NOTIFICATION
                /* Restart peripheral discovery and notification*/
                if (attr_value[0]!=1){
                    perDevs.discovery_enabled = 1;
                    setNewNodesScanning(0x01);
                    //HAL_Delay(300);
                }
#endif
            break;



            //SFUSION_TYPE_ID
            case SFUSION_TYPE_ID:
                
                    if (readCompleted == 0){
                        notificationPending = 1;        
                
                        /* Fill the temp struct */
                        notifyP->att_data     = att_data;
                        notifyP->attr_value   = attr_value;
                        notifyP->conn_handle  = conn_handle;
                        notifyP->i            = i;
                        notifyP->feature_mask = FEATURE_MASK_SENSORFUSION;
                        notifyP->frequency    = SENDING_INTERVAL_100MS_MULTIPLE;
            
                        /* Then, call the Change_Notification_Status from the readCallback with these params */
            
                    }else if (readCompleted == 1) {
                        //notificationPending = 0;
                        Change_Notification_Status(att_data, attr_value, conn_handle, i, FEATURE_MASK_SENSORFUSION, SENDING_INTERVAL_100MS_MULTIPLE);
                    }
                
                    perDevs.sfusion_on[i] = attr_value[0];
                    printf("\r\nSFUSION notifications [%d] %s (status %d)\n", i, (attr_value[0]==1 ? "ON" : "OFF"), perDevs.status);

#if SCAN_AFTER_NOTIFICATION
                /* Restart peripheral discovery and notification*/
                if (attr_value[0]!=1){
                    perDevs.discovery_enabled = 1;
                    setNewNodesScanning(0x01);
                    //HAL_Delay(300);

                }
#endif
            break;
            }
        }//if
    }//if-main


}
/*----------------------------------------------------------------------------*/




/* Retrieve the device index from the peripheral device address */
uint8_t Get_Device_Index_From_Addr(uint8_t *addr){

    uint8_t i;

    for (i=0; i<MAX_NUM_OF_NODES; i++) {

        if ((perDevs.devInfo[i].bdaddr[NODE_ID_B2] == addr[0]) && (perDevs.devInfo[i].bdaddr[NODE_ID_B1] == addr[1])) {
            return i;
        }
    }
    return MAX_NUM_OF_NODES;
}
/*----------------------------------------------------------------------------*/




/**
 * @brief  This function is called to Enable/Disable MIC/PRX/AGM/SFusion notifications
 * @param  att_data : pointer to the modified attribute data
 * @param  connection handle
 * @param  peripheral device index
 * @param  feature mask
 * @param  notification frequency
 * @retval None
 */
 void Change_Notification_Status(uint8_t *att_data, uint8_t  *attr_value, uint16_t conn_handle, uint8_t i,
                                 uint32_t feature_mask, uint8_t frequency) {
    //printf("\r\nChange_Notification_Status (status: %d) (read: %s) (write: %s)\n", perDevs.status, (readCompleted == 1 ? "completed" : "pending"),(writeDescriptorCompleted == 1 ? "completed" : "pending"));//DEBUG
    

        uint8_t  value_len;
        uint16_t attr_handle;


        if (att_data[3] == 1) {

            /* Setting notification frequency (wait of 2s for writing op)*/
            setNotificationProperty(conn_handle, i, feature_mask, NOTIFICATION_FREQ_CMD, frequency);

            /* Stopping the scanning for new peripheral nodes */
            if (perDevs.discovery_enabled) {
                //Disable scanning when notification is enabled
                setNewNodesScanning(0x02);
            }

            /* Disabling all previously enabled notifications */
            disableAllNotifications();

        }//if




        /* Enabling/Disabling notifications */
        value_len = 2;
        attr_value[0] = att_data[3];

        if (feature_mask==FEATURE_MASK_MIC){
            attr_handle = perDevs.mic_char_handle[i] + 2;
            perDevs.mic_event_enabled = attr_value[0];

        } else if (feature_mask==FEATURE_MASK_PROX){
            attr_handle = perDevs.prx_char_handle[i] + 2;
            perDevs.prx_event_enabled = attr_value[0];

        } else if (feature_mask==FEATURE_MASK_ACC){
            attr_handle = perDevs.agm_char_handle[i] + 2;
            perDevs.agm_event_enabled = attr_value[0];

        } else if (feature_mask==FEATURE_MASK_SENSORFUSION){
            attr_handle = perDevs.sfusion_char_handle[i] + 2;
            perDevs.sfusion_event_enabled = attr_value[0];
        }

        
        
            ble_error_t err;
            
            writeDescriptorCompleted=0;
            err = writeCharacDescriptorWithError(conn_handle, attr_handle, value_len, attr_value);

            if (err != BLE_ERROR_NONE){
                writeDescriptorCompleted=1;
                printf("\r\nERROR Enabling/Disabling Notification (err: %d) (stat: %d)\n", err, perDevs.status);
                
            }


    /* WUP notification enable/disable */
    if(attr_value[0]==0x00){
        for (uint8_t j=0; j<(perDevs.connDevices+perDevs.discDevices); j++) {
            if (!perDevs.wup_event[j] && perDevs.devInfo[j].dev_v == NODE_ME1 && perDevs.is_connected[j]) {
                attr_handle = perDevs.wup_char_handle[j] + 2;
                attr_value[0] = 0x01;
    
                setNotificationProperty(perDevs.connection_handle[j], j, FEATURE_MASK_WAKEUP_EVENTS, WAKEUP_NOTIFICATION_CMD, 1);

//                if (discoveryCompleted == 1){                  
                    writeDescriptorCompleted=0;
                    ble_error_t wupErrOn = writeCharacDescriptorWithError(perDevs.connection_handle[j], attr_handle, value_len, attr_value);

                    if (wupErrOn == BLE_ERROR_NONE){
                        perDevs.wup_event_enabled = 1;
                        perDevs.wup_event[j] = 1;
                        printf("\r\nWUP notification on node [%d] ON\n", j);
                    
                    } else {
                        printf("\r\nWrite WUP char descriptor failed! (%d)\n", wupErrOn);
                        writeDescriptorCompleted=1;
                    }//if-else
                    
                                           
//                }//if-else-discovery-completed
            }//if
        }//for


        if (perDevs.wup_event_enabled == 1)
            //printf("\r\nAll WUP notifications turned ON\n");//DEBUG
        perDevs.status = ALL_DATA_READ;


    } else{
        for (uint8_t j=0; j<(perDevs.connDevices+perDevs.discDevices); j++) {
            if ((perDevs.wup_event[j]) && (perDevs.devInfo[j].dev_v == NODE_ME1) && (perDevs.is_connected[j])) {
                attr_handle = perDevs.wup_char_handle[j] + 2;
                attr_value[0] = 0x00;


//                if (discoveryCompleted == 1){               
                    writeDescriptorCompleted=0;
                    ble_error_t wupErrOff = writeCharacDescriptorWithError(perDevs.connection_handle[j], attr_handle, value_len, attr_value);

                    if (wupErrOff == BLE_ERROR_NONE){
                        perDevs.wup_event_enabled = 0;
                        perDevs.wup_event[j] = 0;
                        printf("\r\nWUP notification on node [%d] OFF\n", j);

                    } else {         
                        printf("\r\nWrite WUP char descriptor failed! (%d)\n", wupErrOff);
                        writeDescriptorCompleted=1;
                    }

                    
//                }//if-discovery-completed
            }//if
        }//for
        
        perDevs.status = NOTIFICATIONS_DATA_READ;

    }//if-else-wup

        attr_value[0] = att_data[3];
        perDevs.readDeviceIdx = i;

        notificationPending = 0;
        
        //Return to connectionProcess
        perDevs.status = CONN_INIT;
    
    
    

}
/*----------------------------------------------------------------------------*/