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 >= 2u)){ 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 >= 2u)){ 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 >= 2u)){ 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 >= 1u)){ 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 return; 142 } 143 uint8_t flags = (instance->wheel_revolution_data_supported << CSC_FLAG_WHEEL_REVOLUTION_DATA_SUPPORTED); 144 flags |= (instance->crank_revolution_data_supported << CSC_FLAG_CRANK_REVOLUTION_DATA_SUPPORTED); 145 146 uint8_t value[11]; 147 int pos = 0; 148 149 value[pos++] = flags; 150 if (instance->wheel_revolution_data_supported){ 151 little_endian_store_32(value, pos, instance->cumulative_wheel_revolutions); 152 pos += 4; 153 little_endian_store_16(value, pos, instance->last_wheel_event_time); 154 pos += 2; 155 } 156 157 if (instance->crank_revolution_data_supported){ 158 little_endian_store_16(value, pos, instance->cumulative_crank_revolutions); 159 pos += 2; 160 little_endian_store_16(value, pos, instance->last_crank_event_time); 161 pos += 2; 162 } 163 164 att_server_notify(instance->con_handle, instance->measurement_value_handle, &value[0], pos); 165 } 166 167 static void cycling_speed_and_cadence_service_response_can_send_now(void * context){ 168 cycling_speed_and_cadence_t * instance = (cycling_speed_and_cadence_t *) context; 169 if (!instance){ 170 return; 171 } 172 173 uint8_t value[3 + sizeof(cycling_speed_and_cadence_sensor_location_t)]; 174 int pos = 0; 175 value[pos++] = CSC_OPCODE_RESPONSE_CODE; 176 value[pos++] = instance->request_opcode; 177 value[pos++] = instance->response_value; 178 switch (instance->request_opcode){ 179 case CSC_OPCODE_REQUEST_SUPPORTED_SENSOR_LOCATIONS:{ 180 int loc; 181 for (loc = CSC_SERVICE_SENSOR_LOCATION_OTHER; loc < (CSC_SERVICE_SENSOR_LOCATION_RESERVED-1); loc++){ 182 if (instance->supported_sensor_locations & (1u << loc)){ 183 value[pos++] = loc; 184 } 185 } 186 break; 187 } 188 default: 189 break; 190 } 191 csc_opcode_t temp_request_opcode = instance->request_opcode; 192 instance->request_opcode = CSC_OPCODE_IDLE; 193 194 (void) att_server_indicate(instance->con_handle, instance->control_point_value_handle, &value[0], pos); 195 switch (temp_request_opcode){ 196 case CSC_OPCODE_SET_CUMULATIVE_VALUE: 197 if (instance->response_value != CSC_RESPONSE_VALUE_SUCCESS) break; 198 if (instance->measurement_client_configuration_descriptor_notify){ 199 instance->measurement_callback.callback = &cycling_speed_and_cadence_service_csc_measurement_can_send_now; 200 instance->measurement_callback.context = (void*) instance; 201 att_server_register_can_send_now_callback(&instance->measurement_callback, instance->con_handle); 202 } 203 break; 204 default: 205 break; 206 } 207 } 208 209 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){ 210 UNUSED(con_handle); 211 UNUSED(transaction_mode); 212 UNUSED(offset); 213 UNUSED(buffer_size); 214 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 215 216 if (attribute_handle == instance->measurement_client_configuration_descriptor_handle){ 217 if (buffer_size < 2u){ 218 return ATT_ERROR_INVALID_OFFSET; 219 } 220 instance->measurement_client_configuration_descriptor_notify = little_endian_read_16(buffer, 0); 221 instance->con_handle = con_handle; 222 return 0; 223 } 224 225 if (attribute_handle == instance->control_point_client_configuration_descriptor_handle){ 226 if (buffer_size < 2u){ 227 return ATT_ERROR_INVALID_OFFSET; 228 } 229 instance->control_point_client_configuration_descriptor_indicate = little_endian_read_16(buffer, 0); 230 instance->con_handle = con_handle; 231 return 0; 232 } 233 234 if (attribute_handle == instance->control_point_value_handle){ 235 if (instance->control_point_client_configuration_descriptor_indicate == 0u) return CYCLING_SPEED_AND_CADENCE_ERROR_CODE_CCC_DESCRIPTOR_IMPROPERLY_CONFIGURED; 236 if (instance->request_opcode != CSC_OPCODE_IDLE) return CYCLING_SPEED_AND_CADENCE_ERROR_CODE_PROCEDURE_ALREADY_IN_PROGRESS; 237 238 instance->request_opcode = (csc_opcode_t) buffer[0]; 239 instance->response_value = CSC_RESPONSE_VALUE_SUCCESS; 240 241 switch (instance->request_opcode){ 242 case CSC_OPCODE_SET_CUMULATIVE_VALUE: 243 if (instance->wheel_revolution_data_supported){ 244 instance->cumulative_wheel_revolutions = little_endian_read_32(buffer, 1); 245 break; 246 } 247 instance->response_value = CSC_RESPONSE_VALUE_OPERATION_FAILED; 248 break; 249 case CSC_OPCODE_START_SENSOR_CALIBRATION: 250 break; 251 case CSC_OPCODE_UPDATE_SENSOR_LOCATION: 252 if (instance->multiple_sensor_locations_supported){ 253 cycling_speed_and_cadence_sensor_location_t sensor_location = (cycling_speed_and_cadence_sensor_location_t) buffer[1]; 254 if (sensor_location >= CSC_SERVICE_SENSOR_LOCATION_RESERVED){ 255 instance->response_value = CSC_RESPONSE_VALUE_INVALID_PARAMETER; 256 break; 257 } 258 instance->sensor_location = sensor_location; 259 break; 260 } 261 instance->response_value = CSC_RESPONSE_VALUE_OPERATION_FAILED; 262 break; 263 case CSC_OPCODE_REQUEST_SUPPORTED_SENSOR_LOCATIONS: 264 break; 265 default: 266 instance->response_value = CSC_RESPONSE_VALUE_OP_CODE_NOT_SUPPORTED; 267 break; 268 } 269 270 if (instance->control_point_client_configuration_descriptor_indicate){ 271 instance->control_point_callback.callback = &cycling_speed_and_cadence_service_response_can_send_now; 272 instance->control_point_callback.context = (void*) instance; 273 att_server_register_can_send_now_callback(&instance->control_point_callback, instance->con_handle); 274 } 275 return 0; 276 } 277 278 return 0; 279 } 280 281 void cycling_speed_and_cadence_service_server_init(uint32_t supported_sensor_locations, 282 uint8_t multiple_sensor_locations_supported, uint8_t wheel_revolution_data_supported, uint8_t crank_revolution_data_supported){ 283 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 284 285 instance->wheel_revolution_data_supported = wheel_revolution_data_supported; 286 instance->crank_revolution_data_supported = crank_revolution_data_supported; 287 instance->multiple_sensor_locations_supported = multiple_sensor_locations_supported; 288 instance->supported_sensor_locations = supported_sensor_locations; 289 290 instance->sensor_location = CSC_SERVICE_SENSOR_LOCATION_OTHER; 291 292 // get service handle range 293 uint16_t start_handle = 0; 294 uint16_t end_handle = 0xffff; 295 int service_found = gatt_server_get_get_handle_range_for_service_with_uuid16(ORG_BLUETOOTH_SERVICE_CYCLING_SPEED_AND_CADENCE, &start_handle, &end_handle); 296 btstack_assert(service_found != 0); 297 UNUSED(service_found); 298 299 // // get CSC Mesurement characteristic value handle and client configuration handle 300 instance->measurement_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CSC_MEASUREMENT); 301 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); 302 303 // get CSC Feature characteristic value handle and client configuration handle 304 instance->feature_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_CSC_FEATURE); 305 306 // get Body Sensor Location characteristic value handle and client configuration handle 307 instance->sensor_location_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_SENSOR_LOCATION); 308 309 // get SC Control Point characteristic value handle and client configuration handle 310 instance->control_point_value_handle = gatt_server_get_value_handle_for_characteristic_with_uuid16(start_handle, end_handle, ORG_BLUETOOTH_CHARACTERISTIC_SC_CONTROL_POINT); 311 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); 312 log_info("Measurement value handle 0x%02x", instance->measurement_value_handle); 313 log_info("Measurement Cfg value handle 0x%02x", instance->measurement_client_configuration_descriptor_handle); 314 log_info("Feature value handle 0x%02x", instance->feature_handle); 315 log_info("Sensor location value handle 0x%02x", instance->sensor_location_value_handle); 316 log_info("Control Point value handle 0x%02x", instance->control_point_value_handle); 317 log_info("Control P. Cfg. value handle 0x%02x", instance->control_point_client_configuration_descriptor_handle); 318 319 cycling_speed_and_cadence_service.start_handle = start_handle; 320 cycling_speed_and_cadence_service.end_handle = end_handle; 321 cycling_speed_and_cadence_service.read_callback = &cycling_speed_and_cadence_service_read_callback; 322 cycling_speed_and_cadence_service.write_callback = &cycling_speed_and_cadence_service_write_callback; 323 324 att_server_register_service_handler(&cycling_speed_and_cadence_service); 325 } 326 327 static void cycling_speed_and_cadence_service_calculate_cumulative_wheel_revolutions(int32_t revolutions_change){ 328 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 329 if (revolutions_change < 0){ 330 if (instance->cumulative_wheel_revolutions > -revolutions_change){ 331 instance->cumulative_wheel_revolutions += revolutions_change; 332 } else { 333 instance->cumulative_wheel_revolutions = 0; 334 } 335 } else { 336 if (instance->cumulative_wheel_revolutions < (0xffffffff - revolutions_change)){ 337 instance->cumulative_wheel_revolutions += revolutions_change; 338 } else { 339 instance->cumulative_wheel_revolutions = 0xffffffff; 340 } 341 } 342 } 343 344 static void cycling_speed_and_cadence_service_calculate_cumulative_crank_revolutions(uint16_t revolutions_change){ 345 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 346 347 if (instance->cumulative_crank_revolutions <= (0xffffu - revolutions_change)){ 348 instance->cumulative_crank_revolutions += revolutions_change; 349 } else { 350 instance->cumulative_crank_revolutions = 0xffff; 351 } 352 } 353 354 // 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){ 355 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){ 356 cycling_speed_and_cadence_t * instance = &cycling_speed_and_cadence; 357 358 cycling_speed_and_cadence_service_calculate_cumulative_wheel_revolutions(wheel_revolutions); 359 instance->last_wheel_event_time = last_wheel_event_time; 360 361 cycling_speed_and_cadence_service_calculate_cumulative_crank_revolutions(crank_revolutions); 362 instance->last_crank_event_time = last_crank_event_time; 363 364 if (instance->measurement_client_configuration_descriptor_notify){ 365 instance->measurement_callback.callback = &cycling_speed_and_cadence_service_csc_measurement_can_send_now; 366 instance->measurement_callback.context = (void*) instance; 367 att_server_register_can_send_now_callback(&instance->measurement_callback, instance->con_handle); 368 } 369 } 370