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 MATTHIAS 24 * RINGWALD 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 #include "l2cap.h" 49 50 #include "ble/gatt-service/cycling_speed_and_cadence_service_server.h" 51 52 // error codes from cscs spec 53 #define CSC_ERROR_CODE_PROCEDURE_ALREADY_IN_PROGRESS 0x80 54 #define CSC_ERROR_CODE_CCC_DESCRIPTOR_IMPROPERLY_CONFIGURED 0x81 55 56 typedef enum { 57 CSC_RESPONSE_VALUE_SUCCESS = 1, 58 CSC_RESPONSE_VALUE_OP_CODE_NOT_SUPPORTED, 59 CSC_RESPONSE_VALUE_INVALID_PARAMETER, 60 CSC_RESPONSE_VALUE_OPERATION_FAILED 61 } csc_response_value_t; 62 63 typedef struct { 64 hci_con_handle_t con_handle; 65 66 uint8_t wheel_revolution_data_supported; 67 uint8_t crank_revolution_data_supported; 68 uint8_t multiple_sensor_locations_supported; 69 70 // characteristic: CSC Mesurement 71 uint16_t measurement_value_handle; 72 uint32_t cumulative_wheel_revolutions; 73 uint16_t last_wheel_event_time; // Unit has a resolution of 1/1024s 74 uint16_t cumulative_crank_revolutions; 75 uint16_t last_crank_event_time; // Unit has a resolution of 1/1024s 76 77 // characteristic descriptor: Client Characteristic Configuration 78 uint16_t measurement_client_configuration_descriptor_handle; 79 uint16_t measurement_client_configuration_descriptor_notify; 80 btstack_context_callback_registration_t measurement_callback; 81 82 // sensor locations bitmap 83 uint16_t feature_handle; 84 85 // sensor locations 86 uint16_t sensor_location_value_handle; 87 cycling_speed_and_cadence_sensor_location_t sensor_location; 88 uint32_t supported_sensor_locations; 89 90 // characteristic: Heart Rate Control Point 91 uint16_t control_point_value_handle; 92 uint16_t control_point_client_configuration_descriptor_handle; 93 uint16_t control_point_client_configuration_descriptor_indicate; 94 btstack_context_callback_registration_t control_point_callback; 95 96 csc_opcode_t request_opcode; 97 csc_response_value_t response_value; 98 } cycling_speed_and_cadence_t; 99 100 static att_service_handler_t cycling_speed_and_cadence_service; 101 static cycling_speed_and_cadence_t cycling_speed_and_cadence; 102 103 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){ 104 UNUSED(con_handle); 105 UNUSED(attribute_handle); 106 UNUSED(offset); 107 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 108 109 if (attribute_handle == instance->measurement_client_configuration_descriptor_handle){ 110 if (buffer && buffer_size >= 2){ 111 little_endian_store_16(buffer, 0, instance->measurement_client_configuration_descriptor_notify); 112 } 113 return 2; 114 } 115 116 if (attribute_handle == instance->control_point_client_configuration_descriptor_handle){ 117 if (buffer && buffer_size >= 2){ 118 little_endian_store_16(buffer, 0, instance->control_point_client_configuration_descriptor_indicate); 119 } 120 return 2; 121 } 122 123 if (attribute_handle == instance->feature_handle){ 124 if (buffer && buffer_size >= 2){ 125 uint16_t feature = (instance->wheel_revolution_data_supported << CSC_FLAG_WHEEL_REVOLUTION_DATA_SUPPORTED); 126 feature |= (instance->crank_revolution_data_supported << CSC_FLAG_CRANK_REVOLUTION_DATA_SUPPORTED); 127 feature |= (instance->multiple_sensor_locations_supported << CSC_FLAG_MULTIPLE_SENSOR_LOCATIONS_SUPPORTED); 128 little_endian_store_16(buffer, 0, feature); 129 } 130 return 2; 131 } 132 133 if (attribute_handle == instance->sensor_location_value_handle){ 134 if (buffer && buffer_size >= 1){ 135 buffer[0] = instance->sensor_location; 136 } 137 return 1; 138 } 139 return 0; 140 } 141 142 static void cycling_speed_and_cadence_service_csc_measurement_can_send_now(void * context){ 143 cycling_speed_and_cadence_t * instance = (cycling_speed_and_cadence_t *) context; 144 if (!instance){ 145 printf("instance is null (cycling_speed_and_cadence_service_csc_measurement_can_send_now)\n"); 146 return; 147 } 148 uint8_t flags = (instance->wheel_revolution_data_supported << CSC_FLAG_WHEEL_REVOLUTION_DATA_SUPPORTED); 149 flags |= (instance->crank_revolution_data_supported << CSC_FLAG_CRANK_REVOLUTION_DATA_SUPPORTED); 150 151 uint8_t value[11]; 152 int pos = 0; 153 154 value[pos++] = flags; 155 if (instance->wheel_revolution_data_supported){ 156 little_endian_store_32(value, pos, instance->cumulative_wheel_revolutions); 157 pos += 4; 158 little_endian_store_16(value, pos, instance->last_wheel_event_time); 159 pos += 2; 160 printf("send cumulative 0x%04x\n", instance->cumulative_wheel_revolutions); 161 } 162 163 if (instance->crank_revolution_data_supported){ 164 little_endian_store_16(value, pos, instance->cumulative_crank_revolutions); 165 pos += 2; 166 little_endian_store_16(value, pos, instance->last_crank_event_time); 167 pos += 2; 168 } 169 170 att_server_notify(instance->con_handle, instance->measurement_value_handle, &value[0], pos); 171 } 172 173 static void cycling_speed_and_cadence_service_response_can_send_now(void * context){ 174 cycling_speed_and_cadence_t * instance = (cycling_speed_and_cadence_t *) context; 175 if (!instance){ 176 printf("instance is null (cycling_speed_and_cadence_service_response_can_send_now)\n"); 177 return; 178 } 179 180 uint8_t value[3 + sizeof(cycling_speed_and_cadence_sensor_location_t)]; 181 int pos = 0; 182 value[pos++] = CSC_OPCODE_RESPONSE_CODE; 183 value[pos++] = instance->request_opcode; 184 value[pos++] = instance->response_value; 185 switch (instance->request_opcode){ 186 case CSC_OPCODE_REQUEST_SUPPORTED_SENSOR_LOCATIONS:{ 187 int loc; 188 for (loc = CSC_SERVICE_SENSOR_LOCATION_OTHER; loc < CSC_SERVICE_SENSOR_LOCATION_RESERVED-1; loc++){ 189 if (instance->supported_sensor_locations & (1 << loc)){ 190 value[pos++] = loc; 191 } 192 } 193 break; 194 } 195 default: 196 break; 197 } 198 csc_opcode_t temp_request_opcode = instance->request_opcode; 199 instance->request_opcode = CSC_OPCODE_IDLE; 200 201 printf_hexdump(value, pos); 202 uint8_t status = att_server_indicate(instance->con_handle, instance->control_point_value_handle, &value[0], pos); 203 printf("att_server_indicate status 0x%02x\n", status); 204 switch (temp_request_opcode){ 205 case CSC_OPCODE_SET_CUMULATIVE_VALUE: 206 if (instance->response_value != CSC_RESPONSE_VALUE_SUCCESS) break; 207 if (instance->measurement_client_configuration_descriptor_notify){ 208 instance->measurement_callback.callback = &cycling_speed_and_cadence_service_csc_measurement_can_send_now; 209 instance->measurement_callback.context = (void*) instance; 210 att_server_register_can_send_now_callback(&instance->measurement_callback, instance->con_handle); 211 } 212 break; 213 default: 214 break; 215 } 216 } 217 218 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){ 219 UNUSED(con_handle); 220 UNUSED(transaction_mode); 221 UNUSED(offset); 222 UNUSED(buffer_size); 223 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 224 225 // printf("cycling_speed_and_cadence_service_write_callback: attr handle 0x%02x\n", attribute_handle); 226 if (attribute_handle == instance->measurement_client_configuration_descriptor_handle){ 227 if (buffer_size < 2){ 228 return ATT_ERROR_INVALID_OFFSET; 229 } 230 instance->measurement_client_configuration_descriptor_notify = little_endian_read_16(buffer, 0); 231 instance->con_handle = con_handle; 232 if (instance->measurement_client_configuration_descriptor_notify){ 233 printf("enable notification\n"); 234 } 235 return 0; 236 } 237 238 if (attribute_handle == instance->control_point_client_configuration_descriptor_handle){ 239 if (buffer_size < 2){ 240 return ATT_ERROR_INVALID_OFFSET; 241 } 242 instance->control_point_client_configuration_descriptor_indicate = little_endian_read_16(buffer, 0); 243 instance->con_handle = con_handle; 244 if (instance->control_point_client_configuration_descriptor_indicate){ 245 printf("enable indication\n"); 246 } 247 return 0; 248 } 249 250 if (attribute_handle == instance->control_point_value_handle){ 251 if (instance->control_point_client_configuration_descriptor_indicate == 0) return CSC_ERROR_CODE_CCC_DESCRIPTOR_IMPROPERLY_CONFIGURED; 252 if (instance->request_opcode != CSC_OPCODE_IDLE) return CSC_ERROR_CODE_PROCEDURE_ALREADY_IN_PROGRESS; 253 254 instance->request_opcode = buffer[0]; 255 instance->response_value = CSC_RESPONSE_VALUE_SUCCESS; 256 257 switch (instance->request_opcode){ 258 case CSC_OPCODE_SET_CUMULATIVE_VALUE: 259 if (instance->wheel_revolution_data_supported){ 260 instance->cumulative_wheel_revolutions = little_endian_read_32(buffer, 1); 261 break; 262 } 263 instance->response_value = CSC_RESPONSE_VALUE_OPERATION_FAILED; 264 break; 265 case CSC_OPCODE_START_SENSOR_CALIBRATION: 266 break; 267 case CSC_OPCODE_UPDATE_SENSOR_LOCATION: 268 if (instance->multiple_sensor_locations_supported){ 269 cycling_speed_and_cadence_sensor_location_t sensor_location = buffer[1]; 270 if (sensor_location >= CSC_SERVICE_SENSOR_LOCATION_RESERVED){ 271 instance->response_value = CSC_RESPONSE_VALUE_INVALID_PARAMETER; 272 break; 273 } 274 instance->sensor_location = sensor_location; 275 break; 276 } 277 instance->response_value = CSC_RESPONSE_VALUE_OPERATION_FAILED; 278 break; 279 case CSC_OPCODE_REQUEST_SUPPORTED_SENSOR_LOCATIONS: 280 break; 281 default: 282 instance->response_value = CSC_RESPONSE_VALUE_OP_CODE_NOT_SUPPORTED; 283 break; 284 } 285 printf("control point, opcode %02x, response %02x\n", instance->request_opcode, instance->response_value); 286 287 if (instance->control_point_client_configuration_descriptor_indicate){ 288 instance->control_point_callback.callback = &cycling_speed_and_cadence_service_response_can_send_now; 289 instance->control_point_callback.context = (void*) instance; 290 att_server_register_can_send_now_callback(&instance->control_point_callback, instance->con_handle); 291 } 292 return 0; 293 } 294 295 printf("heart_rate_service_read_callback, not handeled read on handle 0x%02x\n", attribute_handle); 296 return 0; 297 } 298 299 void cycling_speed_and_cadence_service_server_init(uint32_t supported_sensor_locations, 300 uint8_t multiple_sensor_locations_supported, uint8_t wheel_revolution_data_supported, uint8_t crank_revolution_data_supported){ 301 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 302 303 instance->wheel_revolution_data_supported = wheel_revolution_data_supported; 304 instance->crank_revolution_data_supported = crank_revolution_data_supported; 305 instance->multiple_sensor_locations_supported = multiple_sensor_locations_supported; 306 instance->supported_sensor_locations = supported_sensor_locations; 307 308 instance->sensor_location = CSC_SERVICE_SENSOR_LOCATION_OTHER; 309 310 // get service handle range 311 uint16_t start_handle = 0; 312 uint16_t end_handle = 0xffff; 313 int service_found = gatt_server_get_get_handle_range_for_service_with_uuid16(ORG_BLUETOOTH_SERVICE_CYCLING_SPEED_AND_CADENCE, &start_handle, &end_handle); 314 if (!service_found){ 315 printf("no service found\n"); 316 return; 317 } 318 // // get CSC Mesurement characteristic value handle and client configuration handle 319 instance->measurement_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CSC_MEASUREMENT); 320 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); 321 322 // get CSC Feature characteristic value handle and client configuration handle 323 instance->feature_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CSC_FEATURE); 324 325 // get Body Sensor Location characteristic value handle and client configuration handle 326 instance->sensor_location_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_SENSOR_LOCATION); 327 328 // get SC Control Point characteristic value handle and client configuration handle 329 instance->control_point_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_SC_CONTROL_POINT); 330 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); 331 printf("Measurement value handle 0x%02x\n", instance->measurement_value_handle); 332 printf("Measurement Cfg value handle 0x%02x\n", instance->measurement_client_configuration_descriptor_handle); 333 printf("Feature value handle 0x%02x\n", instance->feature_handle); 334 printf("Sensor location value handle 0x%02x\n", instance->sensor_location_value_handle); 335 printf("Control Point value handle 0x%02x\n", instance->control_point_value_handle); 336 printf("Control P. Cfg. value handle 0x%02x\n", instance->control_point_client_configuration_descriptor_handle); 337 338 cycling_speed_and_cadence_service.start_handle = start_handle; 339 cycling_speed_and_cadence_service.end_handle = end_handle; 340 cycling_speed_and_cadence_service.read_callback = &cycling_speed_and_cadence_service_read_callback; 341 cycling_speed_and_cadence_service.write_callback = &cycling_speed_and_cadence_service_write_callback; 342 343 att_server_register_service_handler(&cycling_speed_and_cadence_service); 344 } 345 346 static void cycling_speed_and_cadence_service_calculate_cumulative_wheel_revolutions(int32_t revolutions_change){ 347 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 348 if (revolutions_change < 0){ 349 if (instance->cumulative_wheel_revolutions > -revolutions_change){ 350 instance->cumulative_wheel_revolutions += revolutions_change; 351 } else { 352 instance->cumulative_wheel_revolutions = 0; 353 } 354 } else { 355 if (instance->cumulative_wheel_revolutions < 0xffffffff - revolutions_change){ 356 instance->cumulative_wheel_revolutions += revolutions_change; 357 } else { 358 instance->cumulative_wheel_revolutions = 0xffffffff; 359 } 360 } 361 printf("cumulative 0x%04x, wheel revolution change %d\n", instance->cumulative_wheel_revolutions, revolutions_change); 362 } 363 364 static void cycling_speed_and_cadence_service_calculate_cumulative_crank_revolutions(uint16_t revolutions_change){ 365 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 366 367 if (instance->cumulative_crank_revolutions <= 0xffff - revolutions_change){ 368 instance->cumulative_crank_revolutions += revolutions_change; 369 } else { 370 instance->cumulative_crank_revolutions = 0xffff; 371 } 372 } 373 374 // 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){ 375 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){ 376 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 377 378 cycling_speed_and_cadence_service_calculate_cumulative_wheel_revolutions(wheel_revolutions); 379 instance->last_wheel_event_time = last_wheel_event_time; 380 381 cycling_speed_and_cadence_service_calculate_cumulative_crank_revolutions(crank_revolutions); 382 instance->last_wheel_event_time = last_crank_event_time; 383 384 if (instance->measurement_client_configuration_descriptor_notify){ 385 instance->measurement_callback.callback = &cycling_speed_and_cadence_service_csc_measurement_can_send_now; 386 instance->measurement_callback.context = (void*) instance; 387 // printf("cycling_speed_and_cadence_service_server_update_values instance %p, context %p\n", instance, instance->measurement_callback.context); 388 att_server_register_can_send_now_callback(&instance->measurement_callback, instance->con_handle); 389 } 390 }