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__ "mesh_lower_transport.c" 39 40 #include <stdio.h> 41 #include <stdlib.h> 42 #include <string.h> 43 44 #include "btstack_memory.h" 45 #include "btstack_util.h" 46 47 #include "mesh/beacon.h" 48 #include "mesh/mesh_iv_index_seq_number.h" 49 #include "mesh/mesh_lower_transport.h" 50 #include "mesh/mesh_node.h" 51 #include "mesh/mesh_peer.h" 52 53 #define LOG_LOWER_TRANSPORT 54 55 static void (*higher_layer_handler)( mesh_transport_callback_type_t callback_type, mesh_transport_status_t status, mesh_pdu_t * pdu); 56 57 static void mesh_print_hex(const char * name, const uint8_t * data, uint16_t len){ 58 printf("%-20s ", name); 59 printf_hexdump(data, len); 60 } 61 // static void mesh_print_x(const char * name, uint32_t value){ 62 // printf("%20s: 0x%x", name, (int) value); 63 // } 64 65 // utility 66 67 // Transport PDU Getter 68 uint16_t mesh_transport_nid(mesh_transport_pdu_t * transport_pdu){ 69 return transport_pdu->network_header[0] & 0x7f; 70 } 71 uint16_t mesh_transport_ctl(mesh_transport_pdu_t * transport_pdu){ 72 return transport_pdu->network_header[1] >> 7; 73 } 74 uint16_t mesh_transport_ttl(mesh_transport_pdu_t * transport_pdu){ 75 return transport_pdu->network_header[1] & 0x7f; 76 } 77 uint32_t mesh_transport_seq(mesh_transport_pdu_t * transport_pdu){ 78 return big_endian_read_24(transport_pdu->network_header, 2); 79 } 80 uint32_t mesh_transport_seq_zero(mesh_transport_pdu_t * transport_pdu){ 81 return transport_pdu->seq_zero; 82 } 83 uint16_t mesh_transport_src(mesh_transport_pdu_t * transport_pdu){ 84 return big_endian_read_16(transport_pdu->network_header, 5); 85 } 86 uint16_t mesh_transport_dst(mesh_transport_pdu_t * transport_pdu){ 87 return big_endian_read_16(transport_pdu->network_header, 7); 88 } 89 uint8_t mesh_transport_control_opcode(mesh_transport_pdu_t * transport_pdu){ 90 return transport_pdu->akf_aid_control & 0x7f; 91 } 92 void mesh_transport_set_nid_ivi(mesh_transport_pdu_t * transport_pdu, uint8_t nid_ivi){ 93 transport_pdu->network_header[0] = nid_ivi; 94 } 95 void mesh_transport_set_ctl_ttl(mesh_transport_pdu_t * transport_pdu, uint8_t ctl_ttl){ 96 transport_pdu->network_header[1] = ctl_ttl; 97 } 98 void mesh_transport_set_seq(mesh_transport_pdu_t * transport_pdu, uint32_t seq){ 99 big_endian_store_24(transport_pdu->network_header, 2, seq); 100 } 101 void mesh_transport_set_src(mesh_transport_pdu_t * transport_pdu, uint16_t src){ 102 big_endian_store_16(transport_pdu->network_header, 5, src); 103 } 104 void mesh_transport_set_dest(mesh_transport_pdu_t * transport_pdu, uint16_t dest){ 105 big_endian_store_16(transport_pdu->network_header, 7, dest); 106 } 107 108 // lower transport 109 110 // prototypes 111 112 static void mesh_lower_transport_run(void); 113 static void mesh_lower_transport_outgoing_complete(void); 114 static void mesh_lower_transport_network_pdu_sent(mesh_network_pdu_t *network_pdu); 115 static void mesh_lower_transport_segment_transmission_timeout(btstack_timer_source_t * ts); 116 117 // state 118 static int lower_transport_retry_count; 119 120 // lower transport incoming 121 static btstack_linked_list_t lower_transport_incoming; 122 123 // lower transport ougoing 124 static btstack_linked_list_t lower_transport_outgoing; 125 126 static mesh_transport_pdu_t * lower_transport_outgoing_pdu; 127 static mesh_network_pdu_t * lower_transport_outgoing_segment; 128 static int lower_transport_outgoing_segment_queued; 129 static int lower_transport_outgoing_transmission_timeout; 130 static uint16_t lower_transport_outgoing_seg_o; 131 132 static void mesh_lower_transport_process_segment_acknowledgement_message(mesh_network_pdu_t *network_pdu){ 133 if (lower_transport_outgoing_pdu == NULL) return; 134 135 uint8_t * lower_transport_pdu = mesh_network_pdu_data(network_pdu); 136 uint16_t seq_zero_pdu = big_endian_read_16(lower_transport_pdu, 1) >> 2; 137 uint16_t seq_zero_out = mesh_transport_seq(lower_transport_outgoing_pdu) & 0x1fff; 138 uint32_t block_ack = big_endian_read_32(lower_transport_pdu, 3); 139 140 #ifdef LOG_LOWER_TRANSPORT 141 printf("[+] Segment Acknowledgment message with seq_zero %06x, block_ack %08x - outgoing seq %06x, block_ack %08x\n", 142 seq_zero_pdu, block_ack, seq_zero_out, lower_transport_outgoing_pdu->block_ack); 143 #endif 144 145 if (block_ack == 0){ 146 // If a Segment Acknowledgment message with the BlockAck field set to 0x00000000 is received, 147 // then the Upper Transport PDU shall be immediately cancelled and the higher layers shall be notified that 148 // the Upper Transport PDU has been cancelled. 149 #ifdef LOG_LOWER_TRANSPORT 150 printf("[+] Block Ack == 0 => Abort\n"); 151 #endif 152 mesh_lower_transport_outgoing_complete(); 153 return; 154 } 155 if (seq_zero_pdu != seq_zero_out){ 156 157 #ifdef LOG_LOWER_TRANSPORT 158 printf("[!] Seq Zero doesn't match\n"); 159 #endif 160 return; 161 } 162 163 lower_transport_outgoing_pdu->block_ack &= ~block_ack; 164 #ifdef LOG_LOWER_TRANSPORT 165 printf("[+] Updated block_ack %08x\n", lower_transport_outgoing_pdu->block_ack); 166 #endif 167 168 if (lower_transport_outgoing_pdu->block_ack == 0){ 169 #ifdef LOG_LOWER_TRANSPORT 170 printf("[+] Sent complete\n"); 171 #endif 172 mesh_lower_transport_outgoing_complete(); 173 } 174 } 175 176 static void mesh_lower_transport_process_unsegmented_control_message(mesh_network_pdu_t *network_pdu){ 177 uint8_t * lower_transport_pdu = mesh_network_pdu_data(network_pdu); 178 uint8_t opcode = lower_transport_pdu[0]; 179 180 #ifdef LOG_LOWER_TRANSPORT 181 printf("Unsegmented Control message, outgoing message %p, opcode %x\n", lower_transport_outgoing_pdu, opcode); 182 #endif 183 184 switch (opcode){ 185 case 0: 186 mesh_lower_transport_process_segment_acknowledgement_message(network_pdu); 187 mesh_network_message_processed_by_higher_layer(network_pdu); 188 break; 189 default: 190 higher_layer_handler(MESH_TRANSPORT_PDU_RECEIVED, MESH_TRANSPORT_STATUS_SUCCESS, (mesh_pdu_t *) network_pdu); 191 break; 192 } 193 } 194 195 // ack / incomplete message 196 197 static void mesh_lower_transport_setup_segmented_acknowledge_message(uint8_t * data, uint8_t obo, uint16_t seq_zero, uint32_t block_ack){ 198 // printf("ACK Upper Transport, seq_zero %x\n", seq_zero); 199 data[0] = 0; // SEG = 0, Opcode = 0 200 big_endian_store_16( data, 1, (obo << 15) | (seq_zero << 2) | 0); // OBO, SeqZero, RFU 201 big_endian_store_32( data, 3, block_ack); 202 #ifdef LOG_LOWER_TRANSPORT 203 mesh_print_hex("ACK Upper Transport", data, 7); 204 #endif 205 } 206 207 static void mesh_lower_transport_send_ack(uint16_t netkey_index, uint8_t ttl, uint16_t dest, uint16_t seq_zero, uint32_t block_ack){ 208 // setup ack message 209 uint8_t ack_msg[7]; 210 mesh_lower_transport_setup_segmented_acknowledge_message(ack_msg, 0, seq_zero, block_ack); 211 // 212 // "3.4.5.2: The output filter of the interface connected to advertising or GATT bearers shall drop all messages with TTL value set to 1." 213 // if (ttl <= 1) return 0; 214 215 // TODO: check transport_pdu_len depending on ctl 216 217 // lookup network by netkey_index 218 const mesh_network_key_t * network_key = mesh_network_key_list_get(netkey_index); 219 if (!network_key) return; 220 221 // allocate network_pdu 222 mesh_network_pdu_t * network_pdu = mesh_network_pdu_get(); 223 if (!network_pdu) return; 224 225 // setup network_pdu 226 mesh_network_setup_pdu(network_pdu, netkey_index, network_key->nid, 1, ttl, mesh_sequence_number_next(), mesh_node_get_primary_element_address(), dest, ack_msg, sizeof(ack_msg)); 227 228 // send network_pdu 229 mesh_network_send_pdu(network_pdu); 230 } 231 232 static void mesh_lower_transport_send_ack_for_transport_pdu(mesh_transport_pdu_t *transport_pdu){ 233 uint16_t seq_zero = mesh_transport_seq_zero(transport_pdu); 234 uint8_t ttl = mesh_transport_ttl(transport_pdu); 235 uint16_t dest = mesh_transport_src(transport_pdu); 236 uint16_t netkey_index = transport_pdu->netkey_index; 237 #ifdef LOG_LOWER_TRANSPORT 238 printf("mesh_transport_send_ack_for_transport_pdu %p with netkey_index %x, TTL = %u, SeqZero = %x, SRC = %x, DST = %x\n", 239 transport_pdu, netkey_index, ttl, seq_zero, mesh_node_get_primary_element_address(), dest); 240 #endif 241 mesh_lower_transport_send_ack(netkey_index, ttl, dest, seq_zero, transport_pdu->block_ack); 242 } 243 244 static void mesh_lower_transport_send_ack_for_network_pdu(mesh_network_pdu_t *network_pdu, uint16_t seq_zero, uint32_t block_ack) { 245 uint8_t ttl = mesh_network_ttl(network_pdu); 246 uint16_t dest = mesh_network_src(network_pdu); 247 uint16_t netkey_index = network_pdu->netkey_index; 248 #ifdef LOG_LOWER_TRANSPORT 249 printf("mesh_transport_send_ack_for_network_pdu %p with netkey_index %x, TTL = %u, SeqZero = %x, SRC = %x, DST = %x\n", 250 network_pdu, netkey_index, ttl, seq_zero, mesh_node_get_primary_element_address(), dest); 251 #endif 252 mesh_lower_transport_send_ack(netkey_index, ttl, dest, seq_zero, block_ack); 253 } 254 255 static void mesh_lower_transport_stop_acknowledgment_timer(mesh_transport_pdu_t *transport_pdu){ 256 if (!transport_pdu->acknowledgement_timer_active) return; 257 transport_pdu->acknowledgement_timer_active = 0; 258 btstack_run_loop_remove_timer(&transport_pdu->acknowledgement_timer); 259 } 260 261 static void mesh_lower_transport_stop_incomplete_timer(mesh_transport_pdu_t *transport_pdu){ 262 if (!transport_pdu->incomplete_timer_active) return; 263 transport_pdu->incomplete_timer_active = 0; 264 btstack_run_loop_remove_timer(&transport_pdu->incomplete_timer); 265 } 266 267 // stops timers and updates reassembly engine 268 static void mesh_lower_transport_rx_segmented_message_complete(mesh_transport_pdu_t *transport_pdu){ 269 // set flag 270 transport_pdu->message_complete = 1; 271 // stop timers 272 mesh_lower_transport_stop_acknowledgment_timer(transport_pdu); 273 mesh_lower_transport_stop_incomplete_timer(transport_pdu); 274 // stop reassembly 275 mesh_peer_t * peer = mesh_peer_for_addr(mesh_transport_src(transport_pdu)); 276 if (peer){ 277 peer->transport_pdu = NULL; 278 } 279 } 280 281 static void mesh_lower_transport_rx_ack_timeout(btstack_timer_source_t *ts){ 282 mesh_transport_pdu_t * transport_pdu = (mesh_transport_pdu_t *) btstack_run_loop_get_timer_context(ts); 283 #ifdef LOG_LOWER_TRANSPORT 284 printf("ACK: acknowledgement timer fired for %p, send ACK\n", transport_pdu); 285 #endif 286 transport_pdu->acknowledgement_timer_active = 0; 287 mesh_lower_transport_send_ack_for_transport_pdu(transport_pdu); 288 } 289 290 static void mesh_lower_transport_rx_incomplete_timeout(btstack_timer_source_t *ts){ 291 mesh_transport_pdu_t * transport_pdu = (mesh_transport_pdu_t *) btstack_run_loop_get_timer_context(ts); 292 #ifdef LOG_LOWER_TRANSPORT 293 printf("mesh_transport_rx_incomplete_timeout for %p - give up\n", transport_pdu); 294 #endif 295 mesh_lower_transport_rx_segmented_message_complete(transport_pdu); 296 // free message 297 btstack_memory_mesh_transport_pdu_free(transport_pdu); 298 } 299 300 static void mesh_lower_transport_start_rx_acknowledgment_timer(mesh_transport_pdu_t *transport_pdu, uint32_t timeout){ 301 #ifdef LOG_LOWER_TRANSPORT 302 printf("ACK: start rx ack timer for %p, timeout %u ms\n", transport_pdu, (int) timeout); 303 #endif 304 btstack_run_loop_set_timer(&transport_pdu->acknowledgement_timer, timeout); 305 btstack_run_loop_set_timer_handler(&transport_pdu->acknowledgement_timer, &mesh_lower_transport_rx_ack_timeout); 306 btstack_run_loop_set_timer_context(&transport_pdu->acknowledgement_timer, transport_pdu); 307 btstack_run_loop_add_timer(&transport_pdu->acknowledgement_timer); 308 transport_pdu->acknowledgement_timer_active = 1; 309 } 310 311 static void mesh_lower_transport_tx_restart_segment_transmission_timer(void){ 312 // restart segment transmission timer for unicast dst 313 // - "This timer shall be set to a minimum of 200 + 50 * TTL milliseconds." 314 uint32_t timeout = 200 + 50 * mesh_transport_ttl(lower_transport_outgoing_pdu); 315 if (lower_transport_outgoing_pdu->acknowledgement_timer_active){ 316 btstack_run_loop_remove_timer(&lower_transport_outgoing_pdu->acknowledgement_timer); 317 } 318 319 #ifdef LOG_LOWER_TRANSPORT 320 printf("ACK: start segment transmission timer for %p, timeout %u ms\n", lower_transport_outgoing_pdu, (int) timeout); 321 #endif 322 323 btstack_run_loop_set_timer(&lower_transport_outgoing_pdu->acknowledgement_timer, timeout); 324 btstack_run_loop_set_timer_handler(&lower_transport_outgoing_pdu->acknowledgement_timer, &mesh_lower_transport_segment_transmission_timeout); 325 btstack_run_loop_add_timer(&lower_transport_outgoing_pdu->acknowledgement_timer); 326 lower_transport_outgoing_pdu->acknowledgement_timer_active = 1; 327 } 328 329 static void mesh_lower_transport_restart_incomplete_timer(mesh_transport_pdu_t *transport_pdu, uint32_t timeout, 330 void (*callback)(btstack_timer_source_t *ts)){ 331 #ifdef LOG_LOWER_TRANSPORT 332 printf("RX-(re)start incomplete timer for %p, timeout %u ms\n", transport_pdu, (int) timeout); 333 #endif 334 if (transport_pdu->incomplete_timer_active){ 335 btstack_run_loop_remove_timer(&transport_pdu->incomplete_timer); 336 } 337 btstack_run_loop_set_timer(&transport_pdu->incomplete_timer, timeout); 338 btstack_run_loop_set_timer_handler(&transport_pdu->incomplete_timer, callback); 339 btstack_run_loop_set_timer_context(&transport_pdu->incomplete_timer, transport_pdu); 340 btstack_run_loop_add_timer(&transport_pdu->incomplete_timer); 341 transport_pdu->incomplete_timer_active = 1; 342 } 343 344 static void mesh_lower_transport_outgoing_complete(void){ 345 #ifdef LOG_LOWER_TRANSPORT 346 printf("mesh_lower_transport_outgoing_complete %p, ack timer active %u, incomplete active %u\n", lower_transport_outgoing_pdu, 347 lower_transport_outgoing_pdu->acknowledgement_timer_active, lower_transport_outgoing_pdu->incomplete_timer_active); 348 #endif 349 // stop timers 350 mesh_lower_transport_stop_acknowledgment_timer(lower_transport_outgoing_pdu); 351 mesh_lower_transport_stop_incomplete_timer(lower_transport_outgoing_pdu); 352 // notify upper transport 353 mesh_transport_pdu_t * pdu = lower_transport_outgoing_pdu; 354 lower_transport_outgoing_pdu = NULL; 355 higher_layer_handler(MESH_TRANSPORT_PDU_SENT, MESH_TRANSPORT_STATUS_SEND_ABORT_BY_REMOTE, (mesh_pdu_t *) pdu); 356 } 357 358 static mesh_transport_pdu_t * mesh_lower_transport_pdu_for_segmented_message(mesh_network_pdu_t *network_pdu){ 359 uint16_t src = mesh_network_src(network_pdu); 360 uint16_t seq_zero = ( big_endian_read_16(mesh_network_pdu_data(network_pdu), 1) >> 2) & 0x1fff; 361 #ifdef LOG_LOWER_TRANSPORT 362 printf("mesh_transport_pdu_for_segmented_message: seq_zero %x\n", seq_zero); 363 #endif 364 mesh_peer_t * peer = mesh_peer_for_addr(src); 365 if (!peer) { 366 return NULL; 367 } 368 #ifdef LOG_LOWER_TRANSPORT 369 printf("mesh_seq_zero_validate(%x, %x) -- last (%x, %x)\n", src, seq_zero, peer->address, peer->seq_zero); 370 #endif 371 372 // reception of transport message ongoing 373 if (peer->transport_pdu){ 374 // check if segment for same seq zero 375 uint16_t active_seq_zero = mesh_transport_seq_zero(peer->transport_pdu); 376 if (active_seq_zero == seq_zero) { 377 #ifdef LOG_LOWER_TRANSPORT 378 printf("mesh_transport_pdu_for_segmented_message: segment for current transport pdu with SeqZero %x\n", active_seq_zero); 379 #endif 380 return peer->transport_pdu; 381 } else { 382 // seq zero differs from current transport pdu, but current pdu is not complete 383 #ifdef LOG_LOWER_TRANSPORT 384 printf("mesh_transport_pdu_for_segmented_message: drop segment. current transport pdu SeqZero %x, now %x\n", active_seq_zero, seq_zero); 385 #endif 386 return NULL; 387 } 388 } 389 390 // send ACK if segment for previously completed transport pdu (no ongoing reception, block ack is cleared) 391 if ((seq_zero == peer->seq_zero) && (peer->block_ack != 0)){ 392 #ifdef LOG_LOWER_TRANSPORT 393 printf("mesh_transport_pdu_for_segmented_message: segment for last completed message. send ack\n"); 394 #endif 395 mesh_lower_transport_send_ack_for_network_pdu(network_pdu, seq_zero, peer->block_ack); 396 return NULL; 397 } 398 399 // reconstruct lowest 24 bit of SeqAuth 400 uint32_t seq = mesh_network_seq(network_pdu); 401 uint32_t seq_auth = (seq & 0xffe000) | seq_zero; 402 if (seq_auth > seq){ 403 seq_auth -= 0x2000; 404 } 405 406 // no transport pdu active, check new message: seq auth is greater OR seq auth is same but no segments 407 if (seq_auth > peer->seq_auth || (seq_auth == peer->seq_auth && peer->block_ack == 0)){ 408 mesh_transport_pdu_t * pdu = mesh_transport_pdu_get(); 409 if (!pdu) return NULL; 410 411 // cache network pdu header 412 memcpy(pdu->network_header, network_pdu->data, 9); 413 // store lower 24 bit of SeqAuth for App / Device Nonce 414 big_endian_store_24(pdu->network_header, 2, seq_auth); 415 416 // store meta data in new pdu 417 pdu->netkey_index = network_pdu->netkey_index; 418 pdu->block_ack = 0; 419 pdu->acknowledgement_timer_active = 0; 420 pdu->message_complete = 0; 421 pdu->seq_zero = seq_zero; 422 423 // update peer info 424 peer->transport_pdu = pdu; 425 peer->seq_zero = seq_zero; 426 peer->seq_auth = seq_auth; 427 peer->block_ack = 0; 428 429 #ifdef LOG_LOWER_TRANSPORT 430 printf("mesh_transport_pdu_for_segmented_message: setup transport pdu %p for src %x, seq %06x, seq_zero %x\n", pdu, src, mesh_transport_seq(pdu), seq_zero); 431 #endif 432 return peer->transport_pdu; 433 } else { 434 // seq zero differs from current transport pdu 435 #ifdef LOG_LOWER_TRANSPORT 436 printf("mesh_transport_pdu_for_segmented_message: drop segment for old seq %x\n", seq_zero); 437 #endif 438 return NULL; 439 } 440 } 441 442 static void mesh_lower_transport_process_segment( mesh_transport_pdu_t * transport_pdu, mesh_network_pdu_t * network_pdu){ 443 444 uint8_t * lower_transport_pdu = mesh_network_pdu_data(network_pdu); 445 uint8_t lower_transport_pdu_len = mesh_network_pdu_len(network_pdu); 446 447 // get akf_aid & transmic 448 transport_pdu->akf_aid_control = lower_transport_pdu[0] & 0x7f; 449 transport_pdu->transmic_len = lower_transport_pdu[1] & 0x80 ? 8 : 4; 450 451 // get seq_zero 452 uint16_t seq_zero = ( big_endian_read_16(lower_transport_pdu, 1) >> 2) & 0x1fff; 453 454 // get seg fields 455 uint8_t seg_o = ( big_endian_read_16(lower_transport_pdu, 2) >> 5) & 0x001f; 456 uint8_t seg_n = lower_transport_pdu[3] & 0x1f; 457 uint8_t segment_len = lower_transport_pdu_len - 4; 458 uint8_t * segment_data = &lower_transport_pdu[4]; 459 460 #ifdef LOG_LOWER_TRANSPORT 461 printf("mesh_lower_transport_process_segment: seq zero %04x, seg_o %02x, seg_n %02x, transmic len: %u\n", seq_zero, seg_o, seg_n, transport_pdu->transmic_len * 8); 462 mesh_print_hex("Segment", segment_data, segment_len); 463 #endif 464 465 // store segment 466 memcpy(&transport_pdu->data[seg_o * 12], segment_data, 12); 467 // mark as received 468 transport_pdu->block_ack |= (1<<seg_o); 469 // last segment -> store len 470 if (seg_o == seg_n){ 471 transport_pdu->len = (seg_n * 12) + segment_len; 472 #ifdef LOG_LOWER_TRANSPORT 473 printf("Assembled payload len %u\n", transport_pdu->len); 474 #endif 475 } 476 477 // check for complete 478 int i; 479 for (i=0;i<=seg_n;i++){ 480 if ( (transport_pdu->block_ack & (1<<i)) == 0) return; 481 } 482 483 #ifdef LOG_LOWER_TRANSPORT 484 mesh_print_hex("Assembled payload", transport_pdu->data, transport_pdu->len); 485 #endif 486 487 // mark as done 488 mesh_lower_transport_rx_segmented_message_complete(transport_pdu); 489 490 // store block ack in peer info 491 mesh_peer_t * peer = mesh_peer_for_addr(mesh_transport_src(transport_pdu)); 492 // TODO: check if NULL check can be removed 493 if (peer){ 494 peer->block_ack = transport_pdu->block_ack; 495 } 496 497 // send ack 498 mesh_lower_transport_send_ack_for_transport_pdu(transport_pdu); 499 500 // forward to upper transport 501 higher_layer_handler(MESH_TRANSPORT_PDU_RECEIVED, MESH_TRANSPORT_STATUS_SUCCESS, (mesh_pdu_t*) transport_pdu); 502 } 503 504 void mesh_lower_transport_message_processed_by_higher_layer(mesh_pdu_t * pdu){ 505 switch (pdu->pdu_type){ 506 case MESH_PDU_TYPE_NETWORK: 507 mesh_network_message_processed_by_higher_layer((mesh_network_pdu_t *) pdu); 508 break; 509 case MESH_PDU_TYPE_TRANSPORT: 510 mesh_transport_pdu_free((mesh_transport_pdu_t *) pdu); 511 break; 512 default: 513 break; 514 } 515 } 516 517 void mesh_lower_transport_received_message(mesh_network_callback_type_t callback_type, mesh_network_pdu_t *network_pdu){ 518 mesh_peer_t * peer; 519 uint16_t src; 520 uint16_t seq; 521 switch (callback_type){ 522 case MESH_NETWORK_PDU_RECEIVED: 523 src = mesh_network_src(network_pdu); 524 seq = mesh_network_seq(network_pdu); 525 peer = mesh_peer_for_addr(src); 526 #ifdef LOG_LOWER_TRANSPORT 527 printf("Transport: received message. SRC %x, SEQ %x\n", src, seq); 528 #endif 529 // validate seq 530 if (peer && seq > peer->seq){ 531 // track seq 532 peer->seq = seq; 533 // add to list and go 534 btstack_linked_list_add_tail(&lower_transport_incoming, (btstack_linked_item_t *) network_pdu); 535 mesh_lower_transport_run(); 536 } else { 537 // drop packet 538 #ifdef LOG_LOWER_TRANSPORT 539 printf("Transport: drop packet - src/seq auth failed\n"); 540 #endif 541 mesh_network_message_processed_by_higher_layer(network_pdu); 542 } 543 break; 544 case MESH_NETWORK_PDU_SENT: 545 mesh_lower_transport_network_pdu_sent(network_pdu); 546 break; 547 default: 548 break; 549 } 550 } 551 552 static void mesh_lower_transport_setup_segment(mesh_transport_pdu_t *transport_pdu, uint8_t seg_o, mesh_network_pdu_t *network_pdu){ 553 554 int ctl = mesh_transport_ctl(transport_pdu); 555 uint16_t max_segment_len = ctl ? 8 : 12; // control 8 bytes (64 bit NetMic), access 12 bytes (32 bit NetMIC) 556 557 uint32_t seq = mesh_sequence_number_next(); 558 uint16_t seq_zero = mesh_transport_seq(transport_pdu) & 0x01fff; 559 uint8_t seg_n = (transport_pdu->len - 1) / max_segment_len; 560 uint8_t szmic = ((!ctl) && (transport_pdu->transmic_len == 8)) ? 1 : 0; // only 1 for access messages with 64 bit TransMIC 561 uint8_t nid = mesh_transport_nid(transport_pdu); 562 uint8_t ttl = mesh_transport_ttl(transport_pdu); 563 uint16_t src = mesh_transport_src(transport_pdu); 564 uint16_t dest = mesh_transport_dst(transport_pdu); 565 566 // current segment. 567 uint16_t seg_offset = seg_o * max_segment_len; 568 569 uint8_t lower_transport_pdu_data[16]; 570 lower_transport_pdu_data[0] = 0x80 | transport_pdu->akf_aid_control; 571 big_endian_store_24(lower_transport_pdu_data, 1, (szmic << 23) | (seq_zero << 10) | (seg_o << 5) | seg_n); 572 uint16_t segment_len = btstack_min(transport_pdu->len - seg_offset, max_segment_len); 573 memcpy(&lower_transport_pdu_data[4], &transport_pdu->data[seg_offset], segment_len); 574 uint16_t lower_transport_pdu_len = 4 + segment_len; 575 576 mesh_network_setup_pdu(network_pdu, transport_pdu->netkey_index, nid, 0, ttl, seq, src, dest, lower_transport_pdu_data, lower_transport_pdu_len); 577 } 578 579 static void mesh_lower_transport_send_next_segment(void){ 580 if (!lower_transport_outgoing_pdu) return; 581 582 int ctl = mesh_transport_ctl(lower_transport_outgoing_pdu); 583 uint16_t max_segment_len = ctl ? 8 : 12; // control 8 bytes (64 bit NetMic), access 12 bytes (32 bit NetMIC) 584 uint8_t seg_n = (lower_transport_outgoing_pdu->len - 1) / max_segment_len; 585 586 // find next unacknowledged segement 587 while ((lower_transport_outgoing_seg_o <= seg_n) && ((lower_transport_outgoing_pdu->block_ack & (1 << lower_transport_outgoing_seg_o)) == 0)){ 588 lower_transport_outgoing_seg_o++; 589 } 590 591 if (lower_transport_outgoing_seg_o > seg_n){ 592 #ifdef LOG_LOWER_TRANSPORT 593 printf("[+] Lower Transport, send segmented pdu complete (dst %x)\n", mesh_transport_dst(lower_transport_outgoing_pdu)); 594 #endif 595 lower_transport_outgoing_seg_o = 0; 596 597 // done for unicast, ack timer already set, too 598 if (mesh_network_address_unicast(mesh_transport_dst(lower_transport_outgoing_pdu))) return; 599 600 // done, more? 601 if (lower_transport_retry_count == 0){ 602 #ifdef LOG_LOWER_TRANSPORT 603 printf("[+] Lower Transport, message unacknowledged -> free\n"); 604 #endif 605 // notify upper transport 606 mesh_lower_transport_outgoing_complete(); 607 return; 608 } 609 610 // start retry 611 #ifdef LOG_LOWER_TRANSPORT 612 printf("[+] Lower Transport, message unacknowledged retry count %u\n", lower_transport_retry_count); 613 #endif 614 lower_transport_retry_count--; 615 } 616 617 // restart segment transmission timer for unicast dst 618 if (mesh_network_address_unicast(mesh_transport_dst(lower_transport_outgoing_pdu))){ 619 mesh_lower_transport_tx_restart_segment_transmission_timer(); 620 } 621 622 mesh_lower_transport_setup_segment(lower_transport_outgoing_pdu, lower_transport_outgoing_seg_o, 623 lower_transport_outgoing_segment); 624 625 #ifdef LOG_LOWER_TRANSPORT 626 printf("[+] Lower Transport, send segmented pdu: seg_o %x, seg_n %x\n", lower_transport_outgoing_seg_o, seg_n); 627 mesh_print_hex("LowerTransportPDU", &lower_transport_outgoing_segment->data[9], lower_transport_outgoing_segment->len-9); 628 #endif 629 630 // next segment 631 lower_transport_outgoing_seg_o++; 632 633 // send network pdu 634 lower_transport_outgoing_segment_queued = 1; 635 mesh_network_send_pdu(lower_transport_outgoing_segment); 636 } 637 638 static void mesh_lower_transport_setup_sending_segmented_pdus(void){ 639 printf("[+] Lower Transport, send segmented pdu (retry count %u)\n", lower_transport_retry_count); 640 lower_transport_retry_count--; 641 lower_transport_outgoing_seg_o = 0; 642 } 643 644 static void mesh_lower_transport_segment_transmission_fired(void){ 645 // once more? 646 if (lower_transport_retry_count == 0){ 647 printf("[!] Lower transport, send segmented pdu failed, retries exhausted\n"); 648 mesh_lower_transport_outgoing_complete(); 649 return; 650 } 651 652 // send remaining segments again 653 mesh_lower_transport_setup_sending_segmented_pdus(); 654 } 655 656 static void mesh_lower_transport_network_pdu_sent(mesh_network_pdu_t *network_pdu){ 657 // figure out what pdu was sent 658 659 // single segment of segmented message? 660 if (lower_transport_outgoing_segment == network_pdu){ 661 lower_transport_outgoing_segment_queued = 0; 662 if (lower_transport_outgoing_transmission_timeout){ 663 // handle timeout 664 lower_transport_outgoing_transmission_timeout = 0; 665 mesh_lower_transport_segment_transmission_fired(); 666 } 667 // send next segment 668 mesh_lower_transport_send_next_segment(); 669 return; 670 } 671 672 // Segment Acknowledgment message sent by us? 673 if (mesh_network_control(network_pdu) && network_pdu->data[0] == 0){ 674 btstack_memory_mesh_network_pdu_free(network_pdu); 675 return; 676 } 677 678 // other 679 higher_layer_handler(MESH_TRANSPORT_PDU_SENT, MESH_TRANSPORT_STATUS_SUCCESS, (mesh_pdu_t *) network_pdu); 680 } 681 682 static void mesh_lower_transport_setup_block_ack(mesh_transport_pdu_t *transport_pdu){ 683 // setup block ack - set bit for segment to send, will be cleared on ack 684 int ctl = mesh_transport_ctl(transport_pdu); 685 uint16_t max_segment_len = ctl ? 8 : 12; // control 8 bytes (64 bit NetMic), access 12 bytes (32 bit NetMIC) 686 uint8_t seg_n = (transport_pdu->len - 1) / max_segment_len; 687 if (seg_n < 31){ 688 transport_pdu->block_ack = (1 << (seg_n+1)) - 1; 689 } else { 690 transport_pdu->block_ack = 0xffffffff; 691 } 692 } 693 694 void mesh_lower_transport_send_pdu(mesh_pdu_t *pdu){ 695 if (pdu->pdu_type == MESH_PDU_TYPE_NETWORK){ 696 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) pdu; 697 // network pdu without payload = 9 bytes 698 if (network_pdu->len < 9){ 699 printf("too short, %u\n", network_pdu->len); 700 while(1); 701 } 702 } 703 btstack_linked_list_add_tail(&lower_transport_outgoing, (btstack_linked_item_t*) pdu); 704 mesh_lower_transport_run(); 705 } 706 707 static void mesh_lower_transport_segment_transmission_timeout(btstack_timer_source_t * ts){ 708 UNUSED(ts); 709 #ifdef LOG_LOWER_TRANSPORT 710 printf("[+] Lower transport, segment transmission timer fired for %p\n", lower_transport_outgoing_pdu); 711 #endif 712 lower_transport_outgoing_pdu->acknowledgement_timer_active = 0; 713 714 if (lower_transport_outgoing_segment_queued){ 715 lower_transport_outgoing_transmission_timeout = 1; 716 } else { 717 mesh_lower_transport_segment_transmission_fired(); 718 } 719 } 720 721 static void mesh_lower_transport_run(void){ 722 while(!btstack_linked_list_empty(&lower_transport_incoming)){ 723 // get next message 724 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) btstack_linked_list_pop(&lower_transport_incoming); 725 // segmented? 726 if (mesh_network_segmented(network_pdu)){ 727 mesh_transport_pdu_t * transport_pdu = mesh_lower_transport_pdu_for_segmented_message(network_pdu); 728 if (transport_pdu) { 729 // start acknowledgment timer if inactive 730 if (transport_pdu->acknowledgement_timer_active == 0){ 731 // - "The acknowledgment timer shall be set to a minimum of 150 + 50 * TTL milliseconds" 732 uint32_t timeout = 150 + 50 * mesh_network_ttl(network_pdu); 733 mesh_lower_transport_start_rx_acknowledgment_timer(transport_pdu, timeout); 734 } 735 // restart incomplete timer 736 mesh_lower_transport_restart_incomplete_timer(transport_pdu, 10000, &mesh_lower_transport_rx_incomplete_timeout); 737 mesh_lower_transport_process_segment(transport_pdu, network_pdu); 738 } 739 mesh_network_message_processed_by_higher_layer(network_pdu); 740 } else { 741 // control? 742 if (mesh_network_control(network_pdu)){ 743 // unsegmented control message (not encrypted) 744 mesh_lower_transport_process_unsegmented_control_message(network_pdu); 745 } else { 746 // unsegmented access message (encrypted) 747 higher_layer_handler(MESH_TRANSPORT_PDU_RECEIVED, MESH_TRANSPORT_STATUS_SUCCESS, (mesh_pdu_t *) network_pdu); 748 } 749 } 750 } 751 752 // check if outgoing segmented pdu is active 753 if (lower_transport_outgoing_pdu) return; 754 755 while(!btstack_linked_list_empty(&lower_transport_outgoing)) { 756 // get next message 757 mesh_transport_pdu_t * transport_pdu; 758 mesh_network_pdu_t * network_pdu; 759 mesh_pdu_t * pdu = (mesh_pdu_t *) btstack_linked_list_pop(&lower_transport_outgoing); 760 switch (pdu->pdu_type) { 761 case MESH_PDU_TYPE_NETWORK: 762 network_pdu = (mesh_network_pdu_t *) pdu; 763 mesh_network_pdu_set_seq(network_pdu, mesh_sequence_number_next()); 764 mesh_network_send_pdu(network_pdu); 765 break; 766 case MESH_PDU_TYPE_TRANSPORT: 767 transport_pdu = (mesh_transport_pdu_t *) pdu; 768 // start sending segmented pdu 769 lower_transport_retry_count = 2; 770 lower_transport_outgoing_pdu = transport_pdu; 771 lower_transport_outgoing_transmission_timeout = 0; 772 mesh_lower_transport_setup_block_ack(transport_pdu); 773 mesh_lower_transport_setup_sending_segmented_pdus(); 774 mesh_lower_transport_send_next_segment(); 775 776 return; 777 default: 778 break; 779 } 780 } 781 } 782 783 static void mesh_lower_transport_dump_network_pdus(const char *name, btstack_linked_list_t *list){ 784 printf("List: %s:\n", name); 785 btstack_linked_list_iterator_t it; 786 btstack_linked_list_iterator_init(&it, list); 787 while (btstack_linked_list_iterator_has_next(&it)){ 788 mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t*) btstack_linked_list_iterator_next(&it); 789 printf("- %p: ", network_pdu); printf_hexdump(network_pdu->data, network_pdu->len); 790 } 791 } 792 static void mesh_lower_transport_reset_network_pdus(btstack_linked_list_t *list){ 793 while (!btstack_linked_list_empty(list)){ 794 mesh_network_pdu_t * pdu = (mesh_network_pdu_t *) btstack_linked_list_pop(list); 795 btstack_memory_mesh_network_pdu_free(pdu); 796 } 797 } 798 799 void mesh_lower_transport_dump(void){ 800 mesh_lower_transport_dump_network_pdus("lower_transport_incoming", &lower_transport_incoming); 801 } 802 803 void mesh_lower_transport_reset(void){ 804 mesh_lower_transport_reset_network_pdus(&lower_transport_incoming); 805 if (lower_transport_outgoing_pdu){ 806 mesh_transport_pdu_free(lower_transport_outgoing_pdu); 807 lower_transport_outgoing_pdu = NULL; 808 } 809 mesh_network_pdu_free(lower_transport_outgoing_segment); 810 lower_transport_outgoing_segment_queued = 0; 811 lower_transport_outgoing_segment = NULL; 812 } 813 814 void mesh_lower_transport_init(){ 815 // register with network layer 816 mesh_network_set_higher_layer_handler(&mesh_lower_transport_received_message); 817 // allocate network_pdu for segmentation 818 lower_transport_outgoing_segment_queued = 0; 819 lower_transport_outgoing_segment = mesh_network_pdu_get(); 820 } 821 822 void mesh_lower_transport_set_higher_layer_handler(void (*pdu_handler)( mesh_transport_callback_type_t callback_type, mesh_transport_status_t status, mesh_pdu_t * pdu)){ 823 higher_layer_handler = pdu_handler; 824 } 825 826 // buffer pool 827 mesh_transport_pdu_t * mesh_transport_pdu_get(void){ 828 mesh_transport_pdu_t * transport_pdu = btstack_memory_mesh_transport_pdu_get(); 829 if (transport_pdu) { 830 memset(transport_pdu, 0, sizeof(mesh_transport_pdu_t)); 831 transport_pdu->pdu_header.pdu_type = MESH_PDU_TYPE_TRANSPORT; 832 } 833 return transport_pdu; 834 } 835 836 void mesh_transport_pdu_free(mesh_transport_pdu_t * transport_pdu){ 837 btstack_memory_mesh_transport_pdu_free(transport_pdu); 838 } 839