1 /* 2 * Copyright (C) 2019 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__ "mesh_access.c" 39 40 #include <string.h> 41 #include <stdio.h> 42 #include <stdarg.h> 43 44 #include "mesh/mesh_access.h" 45 46 #include "btstack_debug.h" 47 #include "btstack_memory.h" 48 #include "btstack_tlv.h" 49 50 #include "mesh/beacon.h" 51 #include "mesh/mesh_foundation.h" 52 #include "mesh/mesh_iv_index_seq_number.h" 53 #include "mesh/mesh_node.h" 54 #include "mesh/mesh_proxy.h" 55 #include "mesh/mesh_upper_transport.h" 56 #include "mesh/mesh.h" 57 #include "mesh_upper_transport.h" 58 59 #define MEST_TRANSACTION_TIMEOUT_MS 6000 60 61 static void mesh_access_message_process_handler(mesh_pdu_t * pdu); 62 static void mesh_access_upper_transport_handler(mesh_transport_callback_type_t callback_type, mesh_transport_status_t status, mesh_pdu_t * pdu); 63 static const mesh_operation_t * mesh_model_lookup_operation_by_opcode(mesh_model_t * model, uint32_t opcode); 64 65 // receive 66 static uint16_t mesh_access_received_pdu_refcount; 67 68 // acknowledged messages 69 static btstack_linked_list_t mesh_access_acknowledged_messages; 70 static btstack_timer_source_t mesh_access_acknowledged_timer; 71 static int mesh_access_acknowledged_timer_active; 72 73 // Transitions 74 static uint8_t mesh_transaction_id_counter = 0; 75 76 void mesh_access_init(void){ 77 // register with upper transport 78 mesh_upper_transport_register_access_message_handler(&mesh_access_upper_transport_handler); 79 } 80 81 void mesh_access_emit_state_update_bool(btstack_packet_handler_t event_handler, uint8_t element_index, uint32_t model_identifier, 82 model_state_id_t state_identifier, model_state_update_reason_t reason, uint8_t value){ 83 if (event_handler == NULL) return; 84 uint8_t event[14] = {HCI_EVENT_MESH_META, 12, MESH_SUBEVENT_STATE_UPDATE_BOOL}; 85 int pos = 3; 86 event[pos++] = element_index; 87 little_endian_store_32(event, pos, model_identifier); 88 pos += 4; 89 little_endian_store_32(event, pos, (uint32_t)state_identifier); 90 pos += 4; 91 event[pos++] = (uint8_t)reason; 92 event[pos++] = value; 93 (*event_handler)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 94 } 95 96 void mesh_access_emit_state_update_int16(btstack_packet_handler_t event_handler, uint8_t element_index, uint32_t model_identifier, 97 model_state_id_t state_identifier, model_state_update_reason_t reason, int16_t value){ 98 if (event_handler == NULL) return; 99 uint8_t event[15] = {HCI_EVENT_MESH_META, 13, MESH_SUBEVENT_STATE_UPDATE_BOOL}; 100 int pos = 3; 101 event[pos++] = element_index; 102 little_endian_store_32(event, pos, model_identifier); 103 pos += 4; 104 little_endian_store_32(event, pos, (uint32_t)state_identifier); 105 pos += 4; 106 event[pos++] = (uint8_t)reason; 107 little_endian_store_16(event, pos, (uint16_t) value); 108 pos += 2; 109 (*event_handler)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 110 } 111 112 uint8_t mesh_access_acknowledged_message_retransmissions(void){ 113 return 3; 114 } 115 116 uint32_t mesh_access_acknowledged_message_timeout_ms(void){ 117 return 30000; 118 } 119 120 #define MESH_ACCESS_OPCODE_INVALID 0xFFFFFFFFu 121 #define MESH_ACCESS_OPCODE_NOT_SET 0xFFFFFFFEu 122 123 void mesh_access_send_unacknowledged_pdu(mesh_pdu_t * pdu){ 124 ((mesh_upper_transport_pdu_t *) pdu)->ack_opcode = MESH_ACCESS_OPCODE_INVALID; 125 mesh_upper_transport_send_access_pdu(pdu); 126 } 127 128 void mesh_access_send_acknowledged_pdu(mesh_pdu_t * pdu, uint8_t retransmissions, uint32_t ack_opcode){ 129 ((mesh_upper_transport_pdu_t *) pdu)->retransmit_count = retransmissions; 130 ((mesh_upper_transport_pdu_t *) pdu)->ack_opcode = ack_opcode; 131 132 mesh_upper_transport_send_access_pdu(pdu); 133 } 134 135 static void mesh_access_acknowledged_run(btstack_timer_source_t * ts){ 136 UNUSED(ts); 137 138 uint32_t now = btstack_run_loop_get_time_ms(); 139 140 // handle timeouts 141 btstack_linked_list_iterator_t ack_it; 142 btstack_linked_list_iterator_init(&ack_it, &mesh_access_acknowledged_messages); 143 while (btstack_linked_list_iterator_has_next(&ack_it)){ 144 mesh_pdu_t * pdu = (mesh_pdu_t *) btstack_linked_list_iterator_next(&ack_it); 145 uint32_t retransmit_timeout_ms = ((mesh_upper_transport_pdu_t *) pdu)->retransmit_timeout_ms; 146 if (btstack_time_delta(now, retransmit_timeout_ms) >= 0) { 147 // remove from list 148 btstack_linked_list_remove(&mesh_access_acknowledged_messages, (btstack_linked_item_t*) pdu); 149 // retransmit or report failure 150 uint8_t retransmit_count = ((mesh_upper_transport_pdu_t *) pdu)->retransmit_count; 151 if (retransmit_count){ 152 ((mesh_upper_transport_pdu_t *) pdu)->retransmit_count = retransmit_count - 1; 153 mesh_upper_transport_send_access_pdu(pdu); 154 } else { 155 // find correct model and emit error 156 uint16_t src = mesh_pdu_src(pdu); 157 uint16_t dst = mesh_pdu_dst(pdu); 158 mesh_element_t * element = mesh_node_element_for_unicast_address(src); 159 if (element){ 160 // find 161 mesh_model_iterator_t model_it; 162 mesh_model_iterator_init(&model_it, element); 163 while (mesh_model_iterator_has_next(&model_it)){ 164 mesh_model_t * model = mesh_model_iterator_next(&model_it); 165 // find opcode in table 166 uint32_t ack_opcode = ((mesh_upper_transport_pdu_t *) pdu)->ack_opcode; 167 const mesh_operation_t * operation = mesh_model_lookup_operation_by_opcode(model, ack_opcode); 168 if (operation == NULL) continue; 169 if (model->model_packet_handler == NULL) continue; 170 // emit event 171 uint8_t event[13]; 172 event[0] = HCI_EVENT_MESH_META; 173 event[1] = sizeof(event) - 2; 174 event[2] = element->element_index; 175 little_endian_store_32(event, 3, model->model_identifier); 176 little_endian_store_32(event, 7, ack_opcode); 177 little_endian_store_16(event, 11, dst); 178 (*model->model_packet_handler)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 179 } 180 } 181 182 // free 183 mesh_upper_transport_pdu_free(pdu); 184 } 185 } 186 } 187 188 if (mesh_access_acknowledged_timer_active) return; 189 190 // find earliest timeout and set timer 191 btstack_linked_list_iterator_init(&ack_it, &mesh_access_acknowledged_messages); 192 int32_t next_timeout_ms = 0; 193 while (btstack_linked_list_iterator_has_next(&ack_it)){ 194 mesh_pdu_t * pdu = (mesh_pdu_t *) btstack_linked_list_iterator_next(&ack_it); 195 uint32_t retransmit_timeout_ms = ((mesh_upper_transport_pdu_t *) pdu)->retransmit_timeout_ms; 196 int32_t timeout_delta_ms = btstack_time_delta(retransmit_timeout_ms, now); 197 if (next_timeout_ms == 0 || timeout_delta_ms < next_timeout_ms){ 198 next_timeout_ms = timeout_delta_ms; 199 } 200 } 201 202 // set timer 203 if (next_timeout_ms == 0) return; 204 205 btstack_run_loop_set_timer(&mesh_access_acknowledged_timer, next_timeout_ms); 206 btstack_run_loop_set_timer_handler(&mesh_access_acknowledged_timer, mesh_access_acknowledged_run); 207 btstack_run_loop_add_timer(&mesh_access_acknowledged_timer); 208 mesh_access_acknowledged_timer_active = 1; 209 } 210 211 static void mesh_access_acknowledged_received(uint16_t rx_src, uint32_t opcode){ 212 // check if received src matches our dest 213 // free acknowledged messages if we were waiting for this message 214 215 btstack_linked_list_iterator_t ack_it; 216 btstack_linked_list_iterator_init(&ack_it, &mesh_access_acknowledged_messages); 217 while (btstack_linked_list_iterator_has_next(&ack_it)){ 218 mesh_pdu_t * tx_pdu = (mesh_pdu_t *) btstack_linked_list_iterator_next(&ack_it); 219 uint16_t tx_dest = mesh_pdu_dst(tx_pdu); 220 if (tx_dest != rx_src) continue; 221 if (((mesh_upper_transport_pdu_t *) tx_pdu)->ack_opcode != opcode) continue; 222 // got expected response from dest, remove from outgoing messages 223 mesh_upper_transport_pdu_free(tx_pdu); 224 return; 225 } 226 } 227 228 static void mesh_access_upper_transport_handler(mesh_transport_callback_type_t callback_type, mesh_transport_status_t status, mesh_pdu_t * pdu){ 229 UNUSED(status); 230 switch (callback_type){ 231 case MESH_TRANSPORT_PDU_RECEIVED: 232 mesh_access_message_process_handler(pdu); 233 break; 234 case MESH_TRANSPORT_PDU_SENT: 235 // unacknowledged -> free 236 { 237 if (((mesh_upper_transport_pdu_t *) pdu)->ack_opcode == MESH_ACCESS_OPCODE_INVALID){ 238 mesh_upper_transport_pdu_free(pdu); 239 break; 240 } 241 } 242 // setup timeout 243 uint32_t retransmitTimeoutMs = 244 btstack_run_loop_get_time_ms() + mesh_access_acknowledged_message_timeout_ms(); 245 ((mesh_upper_transport_pdu_t *) pdu)->retransmit_timeout_ms = retransmitTimeoutMs; 246 // add to mesh_access_acknowledged_messages 247 btstack_linked_list_add(&mesh_access_acknowledged_messages, (btstack_linked_item_t *) pdu); 248 // update timer 249 mesh_access_acknowledged_run(NULL); 250 break; 251 default: 252 break; 253 } 254 } 255 256 // Mesh Model Transitions 257 258 uint32_t mesh_access_time_gdtt2ms(uint8_t time_gdtt){ 259 uint8_t num_steps = mesh_access_transitions_num_steps_from_gdtt(time_gdtt); 260 if (num_steps > 0x3E) return 0; 261 262 return mesh_access_transitions_step_ms_from_gdtt(time_gdtt) * num_steps; 263 } 264 265 uint8_t mesh_access_transitions_num_steps_from_gdtt(uint8_t time_gdtt){ 266 return time_gdtt & 0x3fu; 267 } 268 269 uint32_t mesh_access_transitions_step_ms_from_gdtt(uint8_t time_gdtt){ 270 mesh_default_transition_step_resolution_t step_resolution = (mesh_default_transition_step_resolution_t) (time_gdtt >> 6); 271 switch (step_resolution){ 272 case MESH_DEFAULT_TRANSITION_STEP_RESOLUTION_100ms: 273 return 100; 274 case MESH_DEFAULT_TRANSITION_STEP_RESOLUTION_1s: 275 return 1000; 276 case MESH_DEFAULT_TRANSITION_STEP_RESOLUTION_10s: 277 return 10000; 278 case MESH_DEFAULT_TRANSITION_STEP_RESOLUTION_10min: 279 return 600000; 280 default: 281 return 0; 282 } 283 } 284 285 uint8_t mesh_access_transactions_get_next_transaction_id(void){ 286 mesh_transaction_id_counter++; 287 if (mesh_transaction_id_counter == 0){ 288 mesh_transaction_id_counter = 1; 289 } 290 return mesh_transaction_id_counter; 291 } 292 293 static int mesh_access_transitions_transaction_is_expired(mesh_transition_t * transition){ 294 return (btstack_run_loop_get_time_ms() - transition->transaction_timestamp_ms) > MEST_TRANSACTION_TIMEOUT_MS; 295 } 296 297 mesh_transaction_status_t mesh_access_transitions_transaction_status(mesh_transition_t * transition, uint8_t transaction_identifier, uint16_t src_address, uint16_t dst_address){ 298 if (transition->src_address != src_address || transition->dst_address != dst_address) return MESH_TRANSACTION_STATUS_DIFFERENT_DST_OR_SRC; 299 300 if (transition->transaction_identifier == transaction_identifier && !mesh_access_transitions_transaction_is_expired(transition)){ 301 return MESH_TRANSACTION_STATUS_RETRANSMISSION; 302 } 303 return MESH_TRANSACTION_STATUS_NEW; 304 } 305 306 void mesh_access_transitions_init_transaction(mesh_transition_t * transition, uint8_t transaction_identifier, uint16_t src_address, uint16_t dst_address){ 307 transition->transaction_timestamp_ms = btstack_run_loop_get_time_ms(); 308 transition->transaction_identifier = transaction_identifier; 309 transition->src_address = src_address; 310 transition->dst_address = dst_address; 311 } 312 313 static void mesh_server_transition_timeout(btstack_timer_source_t * ts){ 314 mesh_transition_t * base_transition = (mesh_transition_t*) btstack_run_loop_get_timer_context(ts); 315 switch (base_transition->state){ 316 case MESH_TRANSITION_STATE_DELAYED: 317 base_transition->state = MESH_TRANSITION_STATE_ACTIVE; 318 (*base_transition->transition_callback)(base_transition, MODEL_STATE_UPDATE_REASON_TRANSITION_START); 319 if (base_transition->num_steps > 0){ 320 btstack_run_loop_set_timer(&base_transition->timer, base_transition->step_duration_ms); 321 btstack_run_loop_add_timer(&base_transition->timer); 322 return; 323 } 324 base_transition->state = MESH_TRANSITION_STATE_IDLE; 325 (*base_transition->transition_callback)(base_transition, MODEL_STATE_UPDATE_REASON_TRANSITION_END); 326 break; 327 case MESH_TRANSITION_STATE_ACTIVE: 328 if (base_transition->num_steps < MESH_TRANSITION_NUM_STEPS_INFINITE){ 329 base_transition->num_steps--; 330 } 331 (*base_transition->transition_callback)(base_transition, MODEL_STATE_UPDATE_REASON_TRANSITION_ACTIVE); 332 if (base_transition->num_steps > 0){ 333 btstack_run_loop_set_timer(&base_transition->timer, base_transition->step_duration_ms); 334 btstack_run_loop_add_timer(&base_transition->timer); 335 return; 336 } 337 base_transition->state = MESH_TRANSITION_STATE_IDLE; 338 (*base_transition->transition_callback)(base_transition, MODEL_STATE_UPDATE_REASON_TRANSITION_END); 339 break; 340 default: 341 break; 342 } 343 } 344 345 void mesh_access_transition_setup(mesh_model_t *mesh_model, mesh_transition_t * base_transition, uint8_t transition_time_gdtt, uint8_t delay_time_gdtt, void (*transition_callback)(mesh_transition_t * base_transition, model_state_update_reason_t event)){ 346 347 base_transition->mesh_model = mesh_model; 348 base_transition->num_steps = mesh_access_transitions_num_steps_from_gdtt(transition_time_gdtt); 349 base_transition->step_resolution = (mesh_default_transition_step_resolution_t) (transition_time_gdtt >> 6); 350 base_transition->step_duration_ms = mesh_access_transitions_step_ms_from_gdtt(transition_time_gdtt); 351 base_transition->transition_callback = transition_callback; 352 353 btstack_run_loop_set_timer_context(&base_transition->timer, base_transition); 354 btstack_run_loop_set_timer_handler(&base_transition->timer, &mesh_server_transition_timeout); 355 356 // delayed 357 if (delay_time_gdtt > 0){ 358 base_transition->state = MESH_TRANSITION_STATE_DELAYED; 359 btstack_run_loop_set_timer(&base_transition->timer, delay_time_gdtt * 5); 360 btstack_run_loop_add_timer(&base_transition->timer); 361 return; 362 } 363 364 // started 365 if (base_transition->num_steps > 0){ 366 base_transition->state = MESH_TRANSITION_STATE_ACTIVE; 367 btstack_run_loop_set_timer(&base_transition->timer, base_transition->step_duration_ms); 368 btstack_run_loop_add_timer(&base_transition->timer); 369 return; 370 } 371 372 // instanteneous update 373 base_transition->state = MESH_TRANSITION_STATE_IDLE; 374 (*transition_callback)(base_transition, MODEL_STATE_UPDATE_REASON_SET); 375 return; 376 } 377 378 void mesh_access_transitions_abort_transaction(mesh_transition_t * base_transition){ 379 btstack_run_loop_add_timer(&base_transition->timer); 380 } 381 382 static uint16_t mesh_access_src(mesh_access_pdu_t * access_pdu){ 383 return access_pdu->src; 384 } 385 386 uint16_t mesh_pdu_ctl(mesh_pdu_t * pdu){ 387 switch (pdu->pdu_type){ 388 case MESH_PDU_TYPE_NETWORK: 389 return mesh_network_control((mesh_network_pdu_t *) pdu); 390 default: 391 btstack_assert(false); 392 return 0; 393 } 394 } 395 396 uint16_t mesh_pdu_ttl(mesh_pdu_t * pdu){ 397 switch (pdu->pdu_type){ 398 case MESH_PDU_TYPE_NETWORK: 399 return mesh_network_ttl((mesh_network_pdu_t *) pdu); 400 default: 401 btstack_assert(false); 402 return 0; 403 } 404 } 405 406 uint16_t mesh_pdu_src(mesh_pdu_t * pdu){ 407 switch (pdu->pdu_type){ 408 case MESH_PDU_TYPE_ACCESS: 409 return mesh_access_src((mesh_access_pdu_t *) pdu); 410 case MESH_PDU_TYPE_NETWORK: 411 return mesh_network_src((mesh_network_pdu_t *) pdu); 412 default: 413 btstack_assert(false); 414 return MESH_ADDRESS_UNSASSIGNED; 415 } 416 } 417 418 uint16_t mesh_pdu_dst(mesh_pdu_t * pdu){ 419 switch (pdu->pdu_type){ 420 case MESH_PDU_TYPE_ACCESS: { 421 return ((mesh_access_pdu_t *) pdu)->dst; 422 } 423 case MESH_PDU_TYPE_NETWORK: 424 return mesh_network_dst((mesh_network_pdu_t *) pdu); 425 case MESH_PDU_TYPE_UPPER_UNSEGMENTED_ACCESS: 426 case MESH_PDU_TYPE_UPPER_SEGMENTED_ACCESS: 427 return ((mesh_upper_transport_pdu_t *) pdu)->dst; 428 default: 429 btstack_assert(false); 430 return MESH_ADDRESS_UNSASSIGNED; 431 } 432 } 433 434 uint16_t mesh_pdu_netkey_index(mesh_pdu_t * pdu){ 435 switch (pdu->pdu_type){ 436 case MESH_PDU_TYPE_ACCESS: 437 return ((mesh_access_pdu_t*) pdu)->netkey_index; 438 case MESH_PDU_TYPE_NETWORK: 439 return ((mesh_network_pdu_t *) pdu)->netkey_index; 440 default: 441 btstack_assert(false); 442 return 0; 443 } 444 } 445 446 uint16_t mesh_pdu_appkey_index(mesh_pdu_t * pdu){ 447 switch (pdu->pdu_type){ 448 case MESH_PDU_TYPE_ACCESS: 449 return ((mesh_access_pdu_t*) pdu)->appkey_index; 450 default: 451 btstack_assert(false); 452 return 0; 453 } 454 } 455 456 uint16_t mesh_pdu_len(mesh_pdu_t * pdu){ 457 switch (pdu->pdu_type){ 458 case MESH_PDU_TYPE_ACCESS: 459 return ((mesh_access_pdu_t*) pdu)->len; 460 case MESH_PDU_TYPE_CONTROL: 461 return ((mesh_control_pdu_t*) pdu)->len; 462 case MESH_PDU_TYPE_NETWORK: 463 return ((mesh_network_pdu_t *) pdu)->len - 10; 464 default: 465 btstack_assert(false); 466 return 0; 467 } 468 } 469 470 uint8_t * mesh_pdu_data(mesh_pdu_t * pdu){ 471 switch (pdu->pdu_type){ 472 case MESH_PDU_TYPE_ACCESS: 473 return ((mesh_access_pdu_t*) pdu)->data; 474 case MESH_PDU_TYPE_CONTROL: 475 return ((mesh_control_pdu_t*) pdu)->data; 476 case MESH_PDU_TYPE_NETWORK: 477 return &((mesh_network_pdu_t *) pdu)->data[10]; 478 default: 479 btstack_assert(false); 480 return NULL; 481 } 482 } 483 484 uint8_t mesh_pdu_control_opcode(mesh_pdu_t * pdu){ 485 switch (pdu->pdu_type){ 486 case MESH_PDU_TYPE_NETWORK: 487 return mesh_network_control_opcode((mesh_network_pdu_t *) pdu); 488 case MESH_PDU_TYPE_CONTROL: 489 return ((mesh_control_pdu_t *) pdu)->akf_aid_control; 490 default: 491 btstack_assert(false); 492 return 0xff; 493 } 494 } 495 496 // message parser 497 498 static int mesh_access_get_opcode(uint8_t * buffer, uint16_t buffer_size, uint32_t * opcode, uint16_t * opcode_size){ 499 switch (buffer[0] >> 6){ 500 case 0: 501 case 1: 502 if (buffer[0] == 0x7f) return 0; 503 *opcode = buffer[0]; 504 *opcode_size = 1; 505 return 1; 506 case 2: 507 if (buffer_size < 2) return 0; 508 *opcode = big_endian_read_16(buffer, 0); 509 *opcode_size = 2; 510 return 1; 511 case 3: 512 if (buffer_size < 3) return 0; 513 *opcode = (buffer[0] << 16) | little_endian_read_16(buffer, 1); 514 *opcode_size = 3; 515 return 1; 516 default: 517 return 0; 518 } 519 } 520 521 int mesh_access_pdu_get_opcode(mesh_pdu_t * pdu, uint32_t * opcode, uint16_t * opcode_size){ 522 btstack_assert(pdu->pdu_type == MESH_PDU_TYPE_ACCESS); 523 return mesh_access_get_opcode(((mesh_access_pdu_t *) pdu)->data, ((mesh_access_pdu_t *) pdu)->len, opcode, 524 opcode_size); 525 } 526 527 void mesh_access_parser_skip(mesh_access_parser_state_t * state, uint16_t bytes_to_skip){ 528 state->data += bytes_to_skip; 529 state->len -= bytes_to_skip; 530 } 531 532 int mesh_access_parser_init(mesh_access_parser_state_t * state, mesh_pdu_t * pdu){ 533 btstack_assert(pdu->pdu_type == MESH_PDU_TYPE_ACCESS); 534 state->data = mesh_pdu_data(pdu); 535 state->len = mesh_pdu_len(pdu); 536 537 uint16_t opcode_size = 0; 538 int ok = mesh_access_get_opcode(state->data, state->len, &state->opcode, &opcode_size); 539 if (ok){ 540 mesh_access_parser_skip(state, opcode_size); 541 } 542 return ok; 543 } 544 545 uint16_t mesh_access_parser_available(mesh_access_parser_state_t * state){ 546 return state->len; 547 } 548 549 uint8_t mesh_access_parser_get_uint8(mesh_access_parser_state_t * state){ 550 uint8_t value = *state->data; 551 mesh_access_parser_skip(state, 1); 552 return value; 553 } 554 555 uint16_t mesh_access_parser_get_uint16(mesh_access_parser_state_t * state){ 556 uint16_t value = little_endian_read_16(state->data, 0); 557 mesh_access_parser_skip(state, 2); 558 return value; 559 } 560 561 uint32_t mesh_access_parser_get_uint24(mesh_access_parser_state_t * state){ 562 uint32_t value = little_endian_read_24(state->data, 0); 563 mesh_access_parser_skip(state, 3); 564 return value; 565 } 566 567 uint32_t mesh_access_parser_get_uint32(mesh_access_parser_state_t * state){ 568 uint32_t value = little_endian_read_32(state->data, 0); 569 mesh_access_parser_skip(state, 4); 570 return value; 571 } 572 573 void mesh_access_parser_get_uint128(mesh_access_parser_state_t * state, uint8_t * dest){ 574 reverse_128( state->data, dest); 575 mesh_access_parser_skip(state, 16); 576 } 577 578 void mesh_access_parser_get_label_uuid(mesh_access_parser_state_t * state, uint8_t * dest){ 579 (void)memcpy(dest, state->data, 16); 580 mesh_access_parser_skip(state, 16); 581 } 582 583 void mesh_access_parser_get_key(mesh_access_parser_state_t * state, uint8_t * dest){ 584 (void)memcpy(dest, state->data, 16); 585 mesh_access_parser_skip(state, 16); 586 } 587 588 uint32_t mesh_access_parser_get_model_identifier(mesh_access_parser_state_t * parser){ 589 uint16_t vendor_id = BLUETOOTH_COMPANY_ID_BLUETOOTH_SIG_INC; 590 if (mesh_access_parser_available(parser) == 4){ 591 vendor_id = mesh_access_parser_get_uint16(parser); 592 } 593 uint16_t model_id = mesh_access_parser_get_uint16(parser); 594 return mesh_model_get_model_identifier(vendor_id, model_id); 595 } 596 597 uint32_t mesh_access_parser_get_sig_model_identifier(mesh_access_parser_state_t * parser){ 598 uint16_t model_id = mesh_access_parser_get_uint16(parser); 599 return mesh_model_get_model_identifier(BLUETOOTH_COMPANY_ID_BLUETOOTH_SIG_INC, model_id); 600 } 601 602 uint32_t mesh_access_parser_get_vendor_model_identifier(mesh_access_parser_state_t * parser){ 603 uint16_t vendor_id = mesh_access_parser_get_uint16(parser); 604 uint16_t model_id = mesh_access_parser_get_uint16(parser); 605 return mesh_model_get_model_identifier(vendor_id, model_id); 606 } 607 608 // Mesh Access Message Builder 609 610 // message builder 611 612 static int mesh_access_setup_opcode(uint8_t * buffer, uint32_t opcode){ 613 if (opcode < 0x100){ 614 buffer[0] = opcode; 615 return 1; 616 } 617 if (opcode < 0x10000){ 618 big_endian_store_16(buffer, 0, opcode); 619 return 2; 620 } 621 buffer[0] = opcode >> 16; 622 little_endian_store_16(buffer, 1, opcode & 0xffff); 623 return 3; 624 } 625 626 // mesh_message_t builder 627 628 void mesh_access_message_init(mesh_upper_transport_builder_t * builder, uint32_t opcode) { 629 mesh_upper_transport_message_init(builder, MESH_PDU_TYPE_UPPER_UNSEGMENTED_ACCESS); 630 631 // add opcode 632 uint8_t opcode_buffer[3]; 633 uint8_t opcode_len = mesh_access_setup_opcode(opcode_buffer, opcode); 634 mesh_access_message_add_data(builder, opcode_buffer, opcode_len); 635 } 636 637 void mesh_access_message_add_data(mesh_upper_transport_builder_t * pdu, const uint8_t * data, uint16_t data_len){ 638 mesh_upper_transport_message_add_data(pdu, data, data_len); 639 } 640 641 void mesh_access_message_add_uint8(mesh_upper_transport_builder_t * builder, uint8_t value){ 642 mesh_upper_transport_message_add_uint8(builder, value); 643 } 644 645 void mesh_access_message_add_uint16(mesh_upper_transport_builder_t * builder, uint16_t value){ 646 mesh_upper_transport_message_add_uint16(builder, value); 647 } 648 649 void mesh_access_message_add_uint24(mesh_upper_transport_builder_t * builder, uint32_t value){ 650 mesh_upper_transport_message_add_uint24(builder, value); 651 } 652 653 void mesh_access_message_add_uint32(mesh_upper_transport_builder_t * builder, uint32_t value){ 654 mesh_upper_transport_message_add_uint32(builder, value); 655 } 656 657 void mesh_access_message_add_model_identifier(mesh_upper_transport_builder_t * builder, uint32_t model_identifier){ 658 if (mesh_model_is_bluetooth_sig(model_identifier)){ 659 return mesh_access_message_add_uint16(builder, mesh_model_get_model_id(model_identifier) ); 660 } else { 661 return mesh_access_message_add_uint32(builder, model_identifier ); 662 } 663 } 664 665 mesh_upper_transport_pdu_t * mesh_access_message_finalize(mesh_upper_transport_builder_t * builder){ 666 mesh_upper_transport_pdu_t * upper_pdu = mesh_upper_transport_message_finalize(builder); 667 if (upper_pdu == NULL) return NULL; 668 669 // upgrade to segmented if needed 670 if (upper_pdu->pdu_header.pdu_type == MESH_PDU_TYPE_UPPER_UNSEGMENTED_ACCESS) { 671 if (((upper_pdu->flags & MESH_TRANSPORT_FLAG_TRANSMIC_64) != 0) || (upper_pdu->len > 11)){ 672 upper_pdu->pdu_header.pdu_type = MESH_PDU_TYPE_UPPER_SEGMENTED_ACCESS; 673 } 674 } 675 return upper_pdu; 676 } 677 678 // access message template 679 mesh_upper_transport_pdu_t * mesh_access_setup_message(const mesh_access_message_t *message_template, ...){ 680 681 mesh_upper_transport_builder_t builder; 682 mesh_access_message_init(&builder, message_template->opcode); 683 684 va_list argptr; 685 va_start(argptr, message_template); 686 687 // add params 688 const char * format = message_template->format; 689 uint16_t word; 690 uint32_t longword; 691 while (*format){ 692 switch (*format){ 693 case '1': 694 word = va_arg(argptr, int); // minimal va_arg is int: 2 bytes on 8+16 bit CPUs 695 mesh_access_message_add_uint8(&builder, word); 696 break; 697 case '2': 698 word = va_arg(argptr, int); // minimal va_arg is int: 2 bytes on 8+16 bit CPUs 699 mesh_access_message_add_uint16(&builder, word); 700 break; 701 case '3': 702 longword = va_arg(argptr, uint32_t); 703 mesh_access_message_add_uint24(&builder, longword); 704 break; 705 case '4': 706 longword = va_arg(argptr, uint32_t); 707 mesh_access_message_add_uint32(&builder, longword); 708 break; 709 case 'm': 710 longword = va_arg(argptr, uint32_t); 711 mesh_access_message_add_model_identifier(&builder, longword); 712 break; 713 default: 714 btstack_assert(false); 715 break; 716 } 717 format++; 718 } 719 720 va_end(argptr); 721 722 return mesh_access_message_finalize(&builder); 723 } 724 725 static const mesh_operation_t * mesh_model_lookup_operation_by_opcode(mesh_model_t * model, uint32_t opcode){ 726 // find opcode in table 727 const mesh_operation_t * operation = model->operations; 728 if (operation == NULL) return NULL; 729 for ( ; operation->handler != NULL ; operation++){ 730 if (operation->opcode != opcode) continue; 731 return operation; 732 } 733 return NULL; 734 } 735 736 static const mesh_operation_t * mesh_model_lookup_operation(mesh_model_t * model, mesh_pdu_t * pdu){ 737 738 uint32_t opcode = 0; 739 uint16_t opcode_size = 0; 740 int ok = mesh_access_pdu_get_opcode( pdu, &opcode, &opcode_size); 741 if (!ok) return NULL; 742 743 uint16_t len = mesh_pdu_len(pdu); 744 745 // find opcode in table 746 const mesh_operation_t * operation = model->operations; 747 if (operation == NULL) return NULL; 748 for ( ; operation->handler != NULL ; operation++){ 749 if (operation->opcode != opcode) continue; 750 if ((opcode_size + operation->minimum_length) > len) continue; 751 return operation; 752 } 753 return NULL; 754 } 755 756 static int mesh_access_validate_appkey_index(mesh_model_t * model, uint16_t appkey_index){ 757 // DeviceKey is valid for all models 758 if (appkey_index == MESH_DEVICE_KEY_INDEX) return 1; 759 // check if AppKey that is bound to this particular model 760 return mesh_model_contains_appkey(model, appkey_index); 761 } 762 763 // decrease use count and report as free if done 764 void mesh_access_message_processed(mesh_pdu_t * pdu){ 765 if (mesh_access_received_pdu_refcount > 0){ 766 mesh_access_received_pdu_refcount--; 767 } 768 if (mesh_access_received_pdu_refcount == 0){ 769 mesh_upper_transport_message_processed_by_higher_layer(pdu); 770 } 771 } 772 773 static void mesh_access_message_process_handler(mesh_pdu_t * pdu){ 774 775 // init use count 776 mesh_access_received_pdu_refcount = 1; 777 778 // get opcode and size 779 uint32_t opcode = 0; 780 uint16_t opcode_size = 0; 781 782 int ok = mesh_access_pdu_get_opcode( pdu, &opcode, &opcode_size); 783 if (!ok) { 784 mesh_access_message_processed(pdu); 785 return; 786 } 787 788 uint16_t len = mesh_pdu_len(pdu); 789 printf("MESH Access Message, Opcode = %x: ", opcode); 790 printf_hexdump(mesh_pdu_data(pdu), len); 791 792 uint16_t src = mesh_pdu_src(pdu); 793 uint16_t dst = mesh_pdu_dst(pdu); 794 uint16_t appkey_index = mesh_pdu_appkey_index(pdu); 795 if (mesh_network_address_unicast(dst)){ 796 // loookup element by unicast address 797 mesh_element_t * element = mesh_node_element_for_unicast_address(dst); 798 if (element != NULL){ 799 // iterate over models, look for operation 800 mesh_model_iterator_t model_it; 801 mesh_model_iterator_init(&model_it, element); 802 while (mesh_model_iterator_has_next(&model_it)){ 803 mesh_model_t * model = mesh_model_iterator_next(&model_it); 804 // find opcode in table 805 const mesh_operation_t * operation = mesh_model_lookup_operation(model, pdu); 806 if (operation == NULL) continue; 807 if (mesh_access_validate_appkey_index(model, appkey_index) == 0) continue; 808 mesh_access_acknowledged_received(src, opcode); 809 mesh_access_received_pdu_refcount++; 810 operation->handler(model, pdu); 811 } 812 } 813 } 814 else if (mesh_network_address_group(dst)){ 815 816 // handle fixed group address 817 if (dst >= 0xff00){ 818 int deliver_to_primary_element = 1; 819 switch (dst){ 820 case MESH_ADDRESS_ALL_PROXIES: 821 if (mesh_foundation_gatt_proxy_get() == 1){ 822 deliver_to_primary_element = 1; 823 } 824 break; 825 case MESH_ADDRESS_ALL_FRIENDS: 826 // TODO: not implemented 827 break; 828 case MESH_ADDRESS_ALL_RELAYS: 829 if (mesh_foundation_relay_get() == 1){ 830 deliver_to_primary_element = 1; 831 } 832 break; 833 case MESH_ADDRESS_ALL_NODES: 834 deliver_to_primary_element = 1; 835 break; 836 default: 837 break; 838 } 839 if (deliver_to_primary_element){ 840 mesh_model_iterator_t model_it; 841 mesh_model_iterator_init(&model_it, mesh_node_get_primary_element()); 842 while (mesh_model_iterator_has_next(&model_it)){ 843 mesh_model_t * model = mesh_model_iterator_next(&model_it); 844 // find opcode in table 845 const mesh_operation_t * operation = mesh_model_lookup_operation(model, pdu); 846 if (operation == NULL) continue; 847 if (mesh_access_validate_appkey_index(model, appkey_index) == 0) continue; 848 mesh_access_acknowledged_received(src, opcode); 849 mesh_access_received_pdu_refcount++; 850 operation->handler(model, pdu); 851 } 852 } 853 } 854 else { 855 // iterate over all elements / models, check subscription list 856 mesh_element_iterator_t it; 857 mesh_element_iterator_init(&it); 858 while (mesh_element_iterator_has_next(&it)){ 859 mesh_element_t * element = (mesh_element_t *) mesh_element_iterator_next(&it); 860 mesh_model_iterator_t model_it; 861 mesh_model_iterator_init(&model_it, element); 862 while (mesh_model_iterator_has_next(&model_it)){ 863 mesh_model_t * model = mesh_model_iterator_next(&model_it); 864 if (mesh_model_contains_subscription(model, dst)){ 865 // find opcode in table 866 const mesh_operation_t * operation = mesh_model_lookup_operation(model, pdu); 867 if (operation == NULL) continue; 868 if (mesh_access_validate_appkey_index(model, appkey_index) == 0) continue; 869 mesh_access_acknowledged_received(src, opcode); 870 mesh_access_received_pdu_refcount++; 871 operation->handler(model, pdu); 872 } 873 } 874 } 875 } 876 } 877 878 // we're done 879 mesh_access_message_processed(pdu); 880 } 881 882 // Mesh Model Publication 883 static btstack_timer_source_t mesh_access_publication_timer; 884 885 static uint32_t mesh_model_publication_retransmit_count(uint8_t retransmit){ 886 return retransmit & 0x07u; 887 } 888 889 static uint32_t mesh_model_publication_retransmission_period_ms(uint8_t retransmit){ 890 return ((uint32_t)((retransmit >> 3) + 1)) * 50; 891 } 892 893 static void mesh_model_publication_setup_publication(mesh_publication_model_t * publication_model, uint32_t now){ 894 895 // set retransmit counter 896 publication_model->retransmit_count = mesh_model_publication_retransmit_count(publication_model->retransmit); 897 898 // schedule next publication or retransmission 899 uint32_t publication_period_ms = mesh_access_time_gdtt2ms(publication_model->period) >> publication_model->period_divisor; 900 901 // set next publication 902 if (publication_period_ms != 0){ 903 publication_model->next_publication_ms = now + publication_period_ms; 904 publication_model->state = MESH_MODEL_PUBLICATION_STATE_W4_PUBLICATION_MS; 905 } else { 906 publication_model->state = MESH_MODEL_PUBLICATION_STATE_IDLE; 907 } 908 } 909 910 // assumes retransmit_count is valid 911 static void mesh_model_publication_setup_retransmission(mesh_publication_model_t * publication_model, uint32_t now){ 912 uint32_t publication_period_ms = mesh_access_time_gdtt2ms(publication_model->period) >> publication_model->period_divisor; 913 914 // retransmission done 915 if (publication_model->retransmit_count == 0) { 916 // wait for next main event if periodic and retransmission complete 917 if (publication_period_ms != 0){ 918 publication_model->state = MESH_MODEL_PUBLICATION_STATE_W4_PUBLICATION_MS; 919 } else { 920 publication_model->state = MESH_MODEL_PUBLICATION_STATE_IDLE; 921 } 922 return; 923 } 924 925 // calc next retransmit time 926 uint32_t retransmission_period_ms = mesh_model_publication_retransmission_period_ms(publication_model->retransmit) >> publication_model->period_divisor; 927 uint32_t retransmission_ms = now + retransmission_period_ms; 928 929 // check next publication timeout is before next retransmission 930 if (publication_period_ms != 0){ 931 if (btstack_time_delta(retransmission_ms, publication_model->next_publication_ms) > 0) return; 932 } 933 934 // schedule next retransmission 935 publication_model->next_retransmit_ms = retransmission_ms; 936 publication_model->state = MESH_MODEL_PUBLICATION_STATE_W4_RETRANSMIT_MS; 937 } 938 939 static void mesh_model_publication_publish_now_model(void * arg){ 940 mesh_model_t * mesh_model = (mesh_model_t *) arg; 941 mesh_publication_model_t * publication_model = mesh_model->publication_model; 942 if (publication_model == NULL) return; 943 if (publication_model->publish_state_fn == NULL) return; 944 uint16_t dest = publication_model->address; 945 if (dest == MESH_ADDRESS_UNSASSIGNED) return; 946 uint16_t appkey_index = publication_model->appkey_index; 947 mesh_transport_key_t * app_key = mesh_transport_key_get(appkey_index); 948 if (app_key == NULL) return; 949 950 // compose message 951 mesh_pdu_t * pdu = (*publication_model->publish_state_fn)(mesh_model); 952 if (pdu == NULL) return; 953 954 // handle ttl = default 955 uint8_t ttl = publication_model->ttl; 956 if (ttl == 0xff){ 957 ttl = mesh_foundation_default_ttl_get(); 958 } 959 960 mesh_upper_transport_setup_access_pdu_header(pdu, app_key->netkey_index, appkey_index, ttl, mesh_access_get_element_address(mesh_model), dest, 0); 961 mesh_access_send_unacknowledged_pdu(pdu); 962 } 963 964 static void mesh_model_trigger_publication(mesh_model_t * mesh_model){ 965 // queue publication 966 mesh_publication_model_t * publication_model = mesh_model->publication_model; 967 publication_model->send_request.context = mesh_model; 968 publication_model->send_request.callback = &mesh_model_publication_publish_now_model; 969 mesh_upper_transport_request_to_send(&publication_model->send_request); 970 } 971 972 static void mesh_model_publication_run(btstack_timer_source_t * ts){ 973 974 uint32_t now = btstack_run_loop_get_time_ms(); 975 976 // iterate over elements and models and handle time-based transitions 977 mesh_element_iterator_t element_it; 978 mesh_element_iterator_init(&element_it); 979 while (mesh_element_iterator_has_next(&element_it)){ 980 mesh_element_t * element = mesh_element_iterator_next(&element_it); 981 mesh_model_iterator_t model_it; 982 mesh_model_iterator_init(&model_it, element); 983 while (mesh_model_iterator_has_next(&model_it)){ 984 mesh_model_t * mesh_model = mesh_model_iterator_next(&model_it); 985 mesh_publication_model_t * publication_model = mesh_model->publication_model; 986 if (publication_model == NULL) continue; 987 988 // check if either timer fired 989 switch (publication_model->state){ 990 case MESH_MODEL_PUBLICATION_STATE_W4_PUBLICATION_MS: 991 if (btstack_time_delta(publication_model->next_publication_ms, now) > 0) break; 992 publication_model->state = MESH_MODEL_PUBLICATION_STATE_PUBLICATION_READY; 993 break; 994 case MESH_MODEL_PUBLICATION_STATE_W4_RETRANSMIT_MS: 995 if (btstack_time_delta(publication_model->next_retransmit_ms, now) > 0) break; 996 publication_model->state = MESH_MODEL_PUBLICATION_STATE_RETRANSMIT_READY; 997 break; 998 default: 999 break; 1000 } 1001 1002 switch (publication_model->state){ 1003 case MESH_MODEL_PUBLICATION_STATE_PUBLICATION_READY: 1004 // schedule next publication and retransmission 1005 mesh_model_publication_setup_publication(publication_model, now); 1006 mesh_model_publication_setup_retransmission(publication_model, now); 1007 mesh_model_trigger_publication(mesh_model); 1008 break; 1009 case MESH_MODEL_PUBLICATION_STATE_RETRANSMIT_READY: 1010 // schedule next retransmission 1011 publication_model->retransmit_count--; 1012 mesh_model_publication_setup_retransmission(publication_model, now); 1013 mesh_model_trigger_publication(mesh_model); 1014 break; 1015 default: 1016 break; 1017 } 1018 } 1019 } 1020 1021 int32_t next_timeout_ms = 0; 1022 mesh_element_iterator_init(&element_it); 1023 while (mesh_element_iterator_has_next(&element_it)){ 1024 mesh_element_t * element = mesh_element_iterator_next(&element_it); 1025 mesh_model_iterator_t model_it; 1026 mesh_model_iterator_init(&model_it, element); 1027 while (mesh_model_iterator_has_next(&model_it)){ 1028 mesh_model_t * mesh_model = mesh_model_iterator_next(&model_it); 1029 mesh_publication_model_t * publication_model = mesh_model->publication_model; 1030 if (publication_model == NULL) continue; 1031 1032 // schedule next 1033 int32_t timeout_delta_ms; 1034 switch (publication_model->state){ 1035 case MESH_MODEL_PUBLICATION_STATE_W4_PUBLICATION_MS: 1036 timeout_delta_ms = btstack_time_delta(publication_model->next_publication_ms, now); 1037 if (next_timeout_ms == 0 || timeout_delta_ms < next_timeout_ms){ 1038 next_timeout_ms = timeout_delta_ms; 1039 } 1040 break; 1041 case MESH_MODEL_PUBLICATION_STATE_W4_RETRANSMIT_MS: 1042 timeout_delta_ms = btstack_time_delta(publication_model->next_retransmit_ms, now); 1043 if (next_timeout_ms == 0 || timeout_delta_ms < next_timeout_ms){ 1044 next_timeout_ms = timeout_delta_ms; 1045 } 1046 break; 1047 default: 1048 break; 1049 } 1050 } 1051 } 1052 1053 // remove current timer if active 1054 if (ts == NULL){ 1055 btstack_run_loop_remove_timer(&mesh_access_publication_timer); 1056 1057 } 1058 1059 // new timeout? 1060 if (next_timeout_ms == 0) return; 1061 1062 // set timer 1063 btstack_run_loop_set_timer(&mesh_access_publication_timer, next_timeout_ms); 1064 btstack_run_loop_set_timer_handler(&mesh_access_publication_timer, mesh_model_publication_run); 1065 btstack_run_loop_add_timer(&mesh_access_publication_timer); 1066 } 1067 1068 void mesh_model_publication_start(mesh_model_t * mesh_model){ 1069 mesh_publication_model_t * publication_model = mesh_model->publication_model; 1070 if (publication_model == NULL) return; 1071 1072 // publish right away 1073 publication_model->state = MESH_MODEL_PUBLICATION_STATE_PUBLICATION_READY; 1074 mesh_model_publication_run(NULL); 1075 } 1076 1077 void mesh_model_publication_stop(mesh_model_t * mesh_model){ 1078 mesh_publication_model_t * publication_model = mesh_model->publication_model; 1079 if (publication_model == NULL) return; 1080 1081 // reset state 1082 publication_model->state = MESH_MODEL_PUBLICATION_STATE_IDLE; 1083 } 1084 1085 void mesh_access_state_changed(mesh_model_t * mesh_model){ 1086 mesh_publication_model_t * publication_model = mesh_model->publication_model; 1087 if (publication_model == NULL) return; 1088 publication_model->state = MESH_MODEL_PUBLICATION_STATE_PUBLICATION_READY; 1089 mesh_model_publication_run(NULL); 1090 } 1091 1092