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