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 uint8_t status = att_server_indicate(instance->con_handle, instance->control_point_value_handle, &value[0], pos); 198 switch (temp_request_opcode){ 199 case CSC_OPCODE_SET_CUMULATIVE_VALUE: 200 if (instance->response_value != CSC_RESPONSE_VALUE_SUCCESS) break; 201 if (instance->measurement_client_configuration_descriptor_notify){ 202 instance->measurement_callback.callback = &cycling_speed_and_cadence_service_csc_measurement_can_send_now; 203 instance->measurement_callback.context = (void*) instance; 204 att_server_register_can_send_now_callback(&instance->measurement_callback, instance->con_handle); 205 } 206 break; 207 default: 208 break; 209 } 210 } 211 212 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){ 213 UNUSED(con_handle); 214 UNUSED(transaction_mode); 215 UNUSED(offset); 216 UNUSED(buffer_size); 217 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 218 219 // printf("cycling_speed_and_cadence_service_write_callback: attr handle 0x%02x\n", attribute_handle); 220 if (attribute_handle == instance->measurement_client_configuration_descriptor_handle){ 221 if (buffer_size < 2){ 222 return ATT_ERROR_INVALID_OFFSET; 223 } 224 instance->measurement_client_configuration_descriptor_notify = little_endian_read_16(buffer, 0); 225 instance->con_handle = con_handle; 226 // if (instance->measurement_client_configuration_descriptor_notify){ 227 // printf("enable notification\n"); 228 // } 229 return 0; 230 } 231 232 if (attribute_handle == instance->control_point_client_configuration_descriptor_handle){ 233 if (buffer_size < 2){ 234 return ATT_ERROR_INVALID_OFFSET; 235 } 236 instance->control_point_client_configuration_descriptor_indicate = little_endian_read_16(buffer, 0); 237 instance->con_handle = con_handle; 238 // if (instance->control_point_client_configuration_descriptor_indicate){ 239 // printf("enable indication\n"); 240 // } 241 return 0; 242 } 243 244 if (attribute_handle == instance->control_point_value_handle){ 245 if (instance->control_point_client_configuration_descriptor_indicate == 0) return CYCLING_SPEED_AND_CADENCE_ERROR_CODE_CCC_DESCRIPTOR_IMPROPERLY_CONFIGURED; 246 if (instance->request_opcode != CSC_OPCODE_IDLE) return CYCLING_SPEED_AND_CADENCE_ERROR_CODE_PROCEDURE_ALREADY_IN_PROGRESS; 247 248 instance->request_opcode = (csc_opcode_t) buffer[0]; 249 instance->response_value = CSC_RESPONSE_VALUE_SUCCESS; 250 251 switch (instance->request_opcode){ 252 case CSC_OPCODE_SET_CUMULATIVE_VALUE: 253 if (instance->wheel_revolution_data_supported){ 254 instance->cumulative_wheel_revolutions = little_endian_read_32(buffer, 1); 255 break; 256 } 257 instance->response_value = CSC_RESPONSE_VALUE_OPERATION_FAILED; 258 break; 259 case CSC_OPCODE_START_SENSOR_CALIBRATION: 260 break; 261 case CSC_OPCODE_UPDATE_SENSOR_LOCATION: 262 if (instance->multiple_sensor_locations_supported){ 263 cycling_speed_and_cadence_sensor_location_t sensor_location = (cycling_speed_and_cadence_sensor_location_t) buffer[1]; 264 if (sensor_location >= CSC_SERVICE_SENSOR_LOCATION_RESERVED){ 265 instance->response_value = CSC_RESPONSE_VALUE_INVALID_PARAMETER; 266 break; 267 } 268 instance->sensor_location = sensor_location; 269 break; 270 } 271 instance->response_value = CSC_RESPONSE_VALUE_OPERATION_FAILED; 272 break; 273 case CSC_OPCODE_REQUEST_SUPPORTED_SENSOR_LOCATIONS: 274 break; 275 default: 276 instance->response_value = CSC_RESPONSE_VALUE_OP_CODE_NOT_SUPPORTED; 277 break; 278 } 279 // printf("control point, opcode %02x, response %02x\n", instance->request_opcode, instance->response_value); 280 281 if (instance->control_point_client_configuration_descriptor_indicate){ 282 instance->control_point_callback.callback = &cycling_speed_and_cadence_service_response_can_send_now; 283 instance->control_point_callback.context = (void*) instance; 284 att_server_register_can_send_now_callback(&instance->control_point_callback, instance->con_handle); 285 } 286 return 0; 287 } 288 289 // printf("heart_rate_service_read_callback, not handeled read on handle 0x%02x\n", attribute_handle); 290 return 0; 291 } 292 293 void cycling_speed_and_cadence_service_server_init(uint32_t supported_sensor_locations, 294 uint8_t multiple_sensor_locations_supported, uint8_t wheel_revolution_data_supported, uint8_t crank_revolution_data_supported){ 295 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 296 297 instance->wheel_revolution_data_supported = wheel_revolution_data_supported; 298 instance->crank_revolution_data_supported = crank_revolution_data_supported; 299 instance->multiple_sensor_locations_supported = multiple_sensor_locations_supported; 300 instance->supported_sensor_locations = supported_sensor_locations; 301 302 instance->sensor_location = CSC_SERVICE_SENSOR_LOCATION_OTHER; 303 304 // get service handle range 305 uint16_t start_handle = 0; 306 uint16_t end_handle = 0xffff; 307 int service_found = gatt_server_get_get_handle_range_for_service_with_uuid16(ORG_BLUETOOTH_SERVICE_CYCLING_SPEED_AND_CADENCE, &start_handle, &end_handle); 308 if (!service_found){ 309 // printf("no service found\n"); 310 return; 311 } 312 // // get CSC Mesurement characteristic value handle and client configuration handle 313 instance->measurement_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CSC_MEASUREMENT); 314 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); 315 316 // get CSC Feature characteristic value handle and client configuration handle 317 instance->feature_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CSC_FEATURE); 318 319 // get Body Sensor Location characteristic value handle and client configuration handle 320 instance->sensor_location_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_SENSOR_LOCATION); 321 322 // get SC Control Point characteristic value handle and client configuration handle 323 instance->control_point_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_SC_CONTROL_POINT); 324 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); 325 log_info("Measurement value handle 0x%02x", instance->measurement_value_handle); 326 log_info("Measurement Cfg value handle 0x%02x", instance->measurement_client_configuration_descriptor_handle); 327 log_info("Feature value handle 0x%02x", instance->feature_handle); 328 log_info("Sensor location value handle 0x%02x", instance->sensor_location_value_handle); 329 log_info("Control Point value handle 0x%02x", instance->control_point_value_handle); 330 log_info("Control P. Cfg. value handle 0x%02x", instance->control_point_client_configuration_descriptor_handle); 331 332 cycling_speed_and_cadence_service.start_handle = start_handle; 333 cycling_speed_and_cadence_service.end_handle = end_handle; 334 cycling_speed_and_cadence_service.read_callback = &cycling_speed_and_cadence_service_read_callback; 335 cycling_speed_and_cadence_service.write_callback = &cycling_speed_and_cadence_service_write_callback; 336 337 att_server_register_service_handler(&cycling_speed_and_cadence_service); 338 } 339 340 static void cycling_speed_and_cadence_service_calculate_cumulative_wheel_revolutions(int32_t revolutions_change){ 341 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 342 if (revolutions_change < 0){ 343 if (instance->cumulative_wheel_revolutions > -revolutions_change){ 344 instance->cumulative_wheel_revolutions += revolutions_change; 345 } else { 346 instance->cumulative_wheel_revolutions = 0; 347 } 348 } else { 349 if (instance->cumulative_wheel_revolutions < (0xffffffff - revolutions_change)){ 350 instance->cumulative_wheel_revolutions += revolutions_change; 351 } else { 352 instance->cumulative_wheel_revolutions = 0xffffffff; 353 } 354 } 355 // printf("cumulative 0x%04x, wheel revolution change %d\n", instance->cumulative_wheel_revolutions, revolutions_change); 356 } 357 358 static void cycling_speed_and_cadence_service_calculate_cumulative_crank_revolutions(uint16_t revolutions_change){ 359 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 360 361 if (instance->cumulative_crank_revolutions <= (0xffff - revolutions_change)){ 362 instance->cumulative_crank_revolutions += revolutions_change; 363 } else { 364 instance->cumulative_crank_revolutions = 0xffff; 365 } 366 } 367 368 // 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){ 369 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){ 370 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 371 372 cycling_speed_and_cadence_service_calculate_cumulative_wheel_revolutions(wheel_revolutions); 373 instance->last_wheel_event_time = last_wheel_event_time; 374 375 cycling_speed_and_cadence_service_calculate_cumulative_crank_revolutions(crank_revolutions); 376 instance->last_wheel_event_time = last_crank_event_time; 377 378 if (instance->measurement_client_configuration_descriptor_notify){ 379 instance->measurement_callback.callback = &cycling_speed_and_cadence_service_csc_measurement_can_send_now; 380 instance->measurement_callback.context = (void*) instance; 381 // printf("cycling_speed_and_cadence_service_server_update_values instance %p, context %p\n", instance, instance->measurement_callback.context); 382 att_server_register_can_send_now_callback(&instance->measurement_callback, instance->con_handle); 383 } 384 } 385