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