1 /* 2 * Copyright (C) 2014 BlueKitchen GmbH 3 * 4 * Redistribution and use in source and binary forms, with or without 5 * modification, are permitted provided that the following conditions 6 * are met: 7 * 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 3. Neither the name of the copyright holders nor the names of 14 * contributors may be used to endorse or promote products derived 15 * from this software without specific prior written permission. 16 * 4. Any redistribution, use, or modification is done solely for 17 * personal benefit and not for any commercial purpose or for 18 * monetary gain. 19 * 20 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS 21 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL BLUEKITCHEN 24 * GMBH OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 30 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31 * SUCH DAMAGE. 32 * 33 * Please inquire about commercial licensing options at 34 * [email protected] 35 * 36 */ 37 38 #define BTSTACK_FILE__ "cycling_speed_and_cadence_service_server.c" 39 40 41 #include "bluetooth.h" 42 #include "btstack_defines.h" 43 #include "ble/att_db.h" 44 #include "ble/att_server.h" 45 #include "btstack_util.h" 46 #include "bluetooth_gatt.h" 47 #include "btstack_debug.h" 48 49 #include "ble/gatt-service/cycling_speed_and_cadence_service_server.h" 50 #include "btstack_event.h" 51 52 typedef enum { 53 CSC_RESPONSE_VALUE_SUCCESS = 1, 54 CSC_RESPONSE_VALUE_OP_CODE_NOT_SUPPORTED, 55 CSC_RESPONSE_VALUE_INVALID_PARAMETER, 56 CSC_RESPONSE_VALUE_OPERATION_FAILED 57 } csc_response_value_t; 58 59 typedef struct { 60 hci_con_handle_t con_handle; 61 62 uint8_t wheel_revolution_data_supported; 63 uint8_t crank_revolution_data_supported; 64 uint8_t multiple_sensor_locations_supported; 65 66 // characteristic: CSC Mesurement 67 uint16_t measurement_value_handle; 68 uint32_t cumulative_wheel_revolutions; 69 uint16_t last_wheel_event_time; // Unit has a resolution of 1/1024s 70 uint16_t cumulative_crank_revolutions; 71 uint16_t last_crank_event_time; // Unit has a resolution of 1/1024s 72 73 // characteristic descriptor: Client Characteristic Configuration 74 uint16_t measurement_client_configuration_descriptor_handle; 75 uint16_t measurement_client_configuration_descriptor_notify; 76 btstack_context_callback_registration_t measurement_callback; 77 78 // sensor locations bitmap 79 uint16_t feature_handle; 80 81 // sensor locations 82 uint16_t sensor_location_value_handle; 83 cycling_speed_and_cadence_sensor_location_t sensor_location; 84 uint32_t supported_sensor_locations; 85 86 // characteristic: Heart Rate Control Point 87 uint16_t control_point_value_handle; 88 uint16_t control_point_client_configuration_descriptor_handle; 89 uint16_t control_point_client_configuration_descriptor_indicate; 90 btstack_context_callback_registration_t control_point_callback; 91 92 csc_opcode_t request_opcode; 93 csc_response_value_t response_value; 94 bool wawiting_for_indication; 95 } cycling_speed_and_cadence_t; 96 97 static att_service_handler_t cycling_speed_and_cadence_service; 98 static cycling_speed_and_cadence_t cycling_speed_and_cadence; 99 100 static void cycling_speed_and_cadence_service_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 101 UNUSED(channel); 102 UNUSED(size); 103 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 104 105 if (packet_type != HCI_EVENT_PACKET) return; 106 107 switch (hci_event_packet_get_type(packet)){ 108 case ATT_EVENT_HANDLE_VALUE_INDICATION_COMPLETE: 109 instance->wawiting_for_indication = false; 110 break; 111 default: 112 break; 113 } 114 } 115 116 static uint16_t cycling_speed_and_cadence_service_read_callback(hci_con_handle_t con_handle, uint16_t attribute_handle, uint16_t offset, uint8_t * buffer, uint16_t buffer_size){ 117 UNUSED(con_handle); 118 UNUSED(attribute_handle); 119 UNUSED(offset); 120 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 121 122 if (attribute_handle == instance->measurement_client_configuration_descriptor_handle){ 123 if (buffer && (buffer_size >= 2u)){ 124 little_endian_store_16(buffer, 0, instance->measurement_client_configuration_descriptor_notify); 125 } 126 return 2; 127 } 128 129 if (attribute_handle == instance->control_point_client_configuration_descriptor_handle){ 130 if (buffer && (buffer_size >= 2u)){ 131 little_endian_store_16(buffer, 0, instance->control_point_client_configuration_descriptor_indicate); 132 } 133 return 2; 134 } 135 136 if (attribute_handle == instance->feature_handle){ 137 if (buffer && (buffer_size >= 2u)){ 138 uint16_t feature = (instance->wheel_revolution_data_supported << CSC_FLAG_WHEEL_REVOLUTION_DATA_SUPPORTED); 139 feature |= (instance->crank_revolution_data_supported << CSC_FLAG_CRANK_REVOLUTION_DATA_SUPPORTED); 140 feature |= (instance->multiple_sensor_locations_supported << CSC_FLAG_MULTIPLE_SENSOR_LOCATIONS_SUPPORTED); 141 little_endian_store_16(buffer, 0, feature); 142 } 143 return 2; 144 } 145 146 if (attribute_handle == instance->sensor_location_value_handle){ 147 if (buffer && (buffer_size >= 1u)){ 148 buffer[0] = instance->sensor_location; 149 } 150 return 1; 151 } 152 return 0; 153 } 154 155 static void cycling_speed_and_cadence_service_csc_measurement_can_send_now(void * context){ 156 cycling_speed_and_cadence_t * instance = (cycling_speed_and_cadence_t *) context; 157 if (!instance){ 158 return; 159 } 160 uint8_t flags = (instance->wheel_revolution_data_supported << CSC_FLAG_WHEEL_REVOLUTION_DATA_SUPPORTED); 161 flags |= (instance->crank_revolution_data_supported << CSC_FLAG_CRANK_REVOLUTION_DATA_SUPPORTED); 162 163 uint8_t value[11]; 164 int pos = 0; 165 166 value[pos++] = flags; 167 if (instance->wheel_revolution_data_supported){ 168 little_endian_store_32(value, pos, instance->cumulative_wheel_revolutions); 169 pos += 4; 170 little_endian_store_16(value, pos, instance->last_wheel_event_time); 171 pos += 2; 172 } 173 174 if (instance->crank_revolution_data_supported){ 175 little_endian_store_16(value, pos, instance->cumulative_crank_revolutions); 176 pos += 2; 177 little_endian_store_16(value, pos, instance->last_crank_event_time); 178 pos += 2; 179 } 180 181 att_server_notify(instance->con_handle, instance->measurement_value_handle, &value[0], pos); 182 } 183 184 static void cycling_speed_and_cadence_service_response_can_send_now(void * context){ 185 cycling_speed_and_cadence_t * instance = (cycling_speed_and_cadence_t *) context; 186 if (!instance){ 187 return; 188 } 189 190 uint8_t value[3 + sizeof(cycling_speed_and_cadence_sensor_location_t)]; 191 int pos = 0; 192 value[pos++] = CSC_OPCODE_RESPONSE_CODE; 193 value[pos++] = instance->request_opcode; 194 value[pos++] = instance->response_value; 195 switch (instance->request_opcode){ 196 case CSC_OPCODE_REQUEST_SUPPORTED_SENSOR_LOCATIONS:{ 197 int loc; 198 for (loc = CSC_SERVICE_SENSOR_LOCATION_OTHER; loc < (CSC_SERVICE_SENSOR_LOCATION_RESERVED-1); loc++){ 199 if (instance->supported_sensor_locations & (1u << loc)){ 200 value[pos++] = loc; 201 } 202 } 203 break; 204 } 205 default: 206 break; 207 } 208 csc_opcode_t temp_request_opcode = instance->request_opcode; 209 instance->request_opcode = CSC_OPCODE_IDLE; 210 211 (void) att_server_indicate(instance->con_handle, instance->control_point_value_handle, &value[0], pos); 212 switch (temp_request_opcode){ 213 case CSC_OPCODE_SET_CUMULATIVE_VALUE: 214 if (instance->response_value != CSC_RESPONSE_VALUE_SUCCESS) break; 215 if (instance->measurement_client_configuration_descriptor_notify){ 216 instance->measurement_callback.callback = &cycling_speed_and_cadence_service_csc_measurement_can_send_now; 217 instance->measurement_callback.context = (void*) instance; 218 att_server_register_can_send_now_callback(&instance->measurement_callback, instance->con_handle); 219 } 220 break; 221 default: 222 break; 223 } 224 } 225 226 static int cycling_speed_and_cadence_service_write_callback(hci_con_handle_t con_handle, uint16_t attribute_handle, uint16_t transaction_mode, uint16_t offset, uint8_t *buffer, uint16_t buffer_size){ 227 UNUSED(con_handle); 228 UNUSED(offset); 229 UNUSED(buffer_size); 230 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 231 232 if (transaction_mode != ATT_TRANSACTION_MODE_NONE){ 233 return 0; 234 } 235 236 if (attribute_handle == instance->measurement_client_configuration_descriptor_handle){ 237 if (buffer_size < 2u){ 238 return ATT_ERROR_INVALID_OFFSET; 239 } 240 instance->measurement_client_configuration_descriptor_notify = little_endian_read_16(buffer, 0); 241 instance->con_handle = con_handle; 242 return 0; 243 } 244 245 if (attribute_handle == instance->control_point_client_configuration_descriptor_handle){ 246 if (buffer_size < 2u){ 247 return ATT_ERROR_INVALID_OFFSET; 248 } 249 instance->control_point_client_configuration_descriptor_indicate = little_endian_read_16(buffer, 0); 250 instance->con_handle = con_handle; 251 return 0; 252 } 253 254 if (attribute_handle == instance->control_point_value_handle){ 255 if (instance->control_point_client_configuration_descriptor_indicate == 0u) return CYCLING_SPEED_AND_CADENCE_ERROR_CODE_CCC_DESCRIPTOR_IMPROPERLY_CONFIGURED; 256 if (instance->request_opcode != CSC_OPCODE_IDLE) return CYCLING_SPEED_AND_CADENCE_ERROR_CODE_PROCEDURE_ALREADY_IN_PROGRESS; 257 if (instance->wawiting_for_indication) return CYCLING_SPEED_AND_CADENCE_ERROR_CODE_PROCEDURE_ALREADY_IN_PROGRESS; 258 259 instance->request_opcode = (csc_opcode_t) buffer[0]; 260 instance->response_value = CSC_RESPONSE_VALUE_SUCCESS; 261 262 switch (instance->request_opcode){ 263 case CSC_OPCODE_SET_CUMULATIVE_VALUE: 264 if (instance->wheel_revolution_data_supported){ 265 instance->cumulative_wheel_revolutions = little_endian_read_32(buffer, 1); 266 break; 267 } 268 instance->response_value = CSC_RESPONSE_VALUE_OPERATION_FAILED; 269 break; 270 case CSC_OPCODE_START_SENSOR_CALIBRATION: 271 break; 272 case CSC_OPCODE_UPDATE_SENSOR_LOCATION: 273 if (instance->multiple_sensor_locations_supported){ 274 cycling_speed_and_cadence_sensor_location_t sensor_location = (cycling_speed_and_cadence_sensor_location_t) buffer[1]; 275 if (sensor_location >= CSC_SERVICE_SENSOR_LOCATION_RESERVED){ 276 instance->response_value = CSC_RESPONSE_VALUE_INVALID_PARAMETER; 277 break; 278 } 279 instance->sensor_location = sensor_location; 280 break; 281 } 282 instance->response_value = CSC_RESPONSE_VALUE_OPERATION_FAILED; 283 break; 284 case CSC_OPCODE_REQUEST_SUPPORTED_SENSOR_LOCATIONS: 285 break; 286 default: 287 instance->response_value = CSC_RESPONSE_VALUE_OP_CODE_NOT_SUPPORTED; 288 break; 289 } 290 291 if (instance->control_point_client_configuration_descriptor_indicate){ 292 instance->control_point_callback.callback = &cycling_speed_and_cadence_service_response_can_send_now; 293 instance->control_point_callback.context = (void*) instance; 294 instance->wawiting_for_indication = true; 295 att_server_request_to_send_indication(&instance->control_point_callback, instance->con_handle); 296 } 297 return 0; 298 } 299 300 return 0; 301 } 302 303 void cycling_speed_and_cadence_service_server_init(uint32_t supported_sensor_locations, 304 uint8_t multiple_sensor_locations_supported, uint8_t wheel_revolution_data_supported, uint8_t crank_revolution_data_supported){ 305 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 306 307 instance->wheel_revolution_data_supported = wheel_revolution_data_supported; 308 instance->crank_revolution_data_supported = crank_revolution_data_supported; 309 instance->multiple_sensor_locations_supported = multiple_sensor_locations_supported; 310 instance->supported_sensor_locations = supported_sensor_locations; 311 312 instance->sensor_location = CSC_SERVICE_SENSOR_LOCATION_OTHER; 313 314 // get service handle range 315 uint16_t start_handle = 0; 316 uint16_t end_handle = 0xffff; 317 int service_found = gatt_server_get_handle_range_for_service_with_uuid16(ORG_BLUETOOTH_SERVICE_CYCLING_SPEED_AND_CADENCE, &start_handle, &end_handle); 318 btstack_assert(service_found != 0); 319 UNUSED(service_found); 320 321 // // get CSC Mesurement characteristic value handle and client configuration handle 322 instance->measurement_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CSC_MEASUREMENT); 323 instance->measurement_client_configuration_descriptor_handle = gatt_server_get_client_configuration_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CSC_MEASUREMENT); 324 325 // get CSC Feature characteristic value handle and client configuration handle 326 instance->feature_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CSC_FEATURE); 327 328 // get Body Sensor Location characteristic value handle and client configuration handle 329 instance->sensor_location_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_SENSOR_LOCATION); 330 331 // get SC Control Point characteristic value handle and client configuration handle 332 instance->control_point_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_SC_CONTROL_POINT); 333 instance->control_point_client_configuration_descriptor_handle = gatt_server_get_client_configuration_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_SC_CONTROL_POINT); 334 log_info("Measurement value handle 0x%02x", instance->measurement_value_handle); 335 log_info("Measurement Cfg value handle 0x%02x", instance->measurement_client_configuration_descriptor_handle); 336 log_info("Feature value handle 0x%02x", instance->feature_handle); 337 log_info("Sensor location value handle 0x%02x", instance->sensor_location_value_handle); 338 log_info("Control Point value handle 0x%02x", instance->control_point_value_handle); 339 log_info("Control P. Cfg. value handle 0x%02x", instance->control_point_client_configuration_descriptor_handle); 340 341 cycling_speed_and_cadence_service.start_handle = start_handle; 342 cycling_speed_and_cadence_service.end_handle = end_handle; 343 cycling_speed_and_cadence_service.read_callback = &cycling_speed_and_cadence_service_read_callback; 344 cycling_speed_and_cadence_service.write_callback = &cycling_speed_and_cadence_service_write_callback; 345 cycling_speed_and_cadence_service.packet_handler = &cycling_speed_and_cadence_service_packet_handler; 346 347 att_server_register_service_handler(&cycling_speed_and_cadence_service); 348 } 349 350 static void cycling_speed_and_cadence_service_calculate_cumulative_wheel_revolutions(int32_t revolutions_change){ 351 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 352 if (revolutions_change < 0){ 353 uint32_t revolutions_to_subtract = (uint32_t)(-revolutions_change); 354 if (instance->cumulative_wheel_revolutions > revolutions_to_subtract){ 355 instance->cumulative_wheel_revolutions -= revolutions_to_subtract; 356 } else { 357 instance->cumulative_wheel_revolutions = 0; 358 } 359 } else { 360 if (instance->cumulative_wheel_revolutions < (0xffffffff - revolutions_change)){ 361 instance->cumulative_wheel_revolutions += revolutions_change; 362 } else { 363 instance->cumulative_wheel_revolutions = 0xffffffff; 364 } 365 } 366 } 367 368 static void cycling_speed_and_cadence_service_calculate_cumulative_crank_revolutions(uint16_t revolutions_change){ 369 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 370 371 if (instance->cumulative_crank_revolutions <= (0xffffu - revolutions_change)){ 372 instance->cumulative_crank_revolutions += revolutions_change; 373 } else { 374 instance->cumulative_crank_revolutions = 0xffff; 375 } 376 } 377 378 // The Cumulative Wheel Revolutions value may decrement (e.g. If the bicycle is rolled in reverse), but shall not decrease below 0void cycling_speed_and_cadence_service_add_wheel_revolutions(int32_t wheel_revolutions, uint16_t last_wheel_event_time){ 379 void cycling_speed_and_cadence_service_server_update_values(int32_t wheel_revolutions, uint16_t last_wheel_event_time, uint16_t crank_revolutions, uint16_t last_crank_event_time){ 380 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 381 382 cycling_speed_and_cadence_service_calculate_cumulative_wheel_revolutions(wheel_revolutions); 383 instance->last_wheel_event_time = last_wheel_event_time; 384 385 cycling_speed_and_cadence_service_calculate_cumulative_crank_revolutions(crank_revolutions); 386 instance->last_crank_event_time = last_crank_event_time; 387 388 if (instance->measurement_client_configuration_descriptor_notify){ 389 instance->measurement_callback.callback = &cycling_speed_and_cadence_service_csc_measurement_can_send_now; 390 instance->measurement_callback.context = (void*) instance; 391 att_server_register_can_send_now_callback(&instance->measurement_callback, instance->con_handle); 392 } 393 } 394