1 /* 2 * Copyright (C) 2017 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__ "le_device_db_tlv.c" 39 40 #include "ble/le_device_db.h" 41 #include "ble/le_device_db_tlv.h" 42 43 #include "ble/core.h" 44 45 #include <stdio.h> 46 #include <string.h> 47 #include "btstack_debug.h" 48 49 // LE Device DB Implementation storing entries in btstack_tlv 50 51 // Local cache is used to keep track of deleted entries in TLV 52 53 #define INVALID_ENTRY_ADDR_TYPE 0xff 54 55 // Single stored entry 56 typedef struct le_device_db_entry_t { 57 58 uint32_t seq_nr; // used for "least recently stored" eviction strategy 59 60 // Identification 61 int addr_type; 62 bd_addr_t addr; 63 sm_key_t irk; 64 65 // Stored pairing information allows to re-establish an enncrypted connection 66 // with a peripheral that doesn't have any persistent memory 67 sm_key_t ltk; 68 uint16_t ediv; 69 uint8_t rand[8]; 70 uint8_t key_size; 71 uint8_t authenticated; 72 uint8_t authorized; 73 74 #ifdef ENABLE_LE_SIGNED_WRITE 75 // Signed Writes by remote 76 sm_key_t remote_csrk; 77 uint32_t remote_counter; 78 79 // Signed Writes by us 80 sm_key_t local_csrk; 81 uint32_t local_counter; 82 #endif 83 84 } le_device_db_entry_t; 85 86 87 #ifndef NVM_NUM_DEVICE_DB_ENTRIES 88 #error "NVM_NUM_DEVICE_DB_ENTRIES not defined, please define in btstack_config.h" 89 #endif 90 91 #if NVM_NUM_DEVICE_DB_ENTRIES == 0 92 #error "NVM_NUM_DEVICE_DB_ENTRIES must not be 0, please update in btstack_config.h" 93 #endif 94 95 // only stores if entry present 96 static uint8_t entry_map[NVM_NUM_DEVICE_DB_ENTRIES]; 97 static uint32_t num_valid_entries; 98 99 static const btstack_tlv_t * le_device_db_tlv_btstack_tlv_impl; 100 static void * le_device_db_tlv_btstack_tlv_context; 101 102 static const char tag_0 = 'B'; 103 static const char tag_1 = 'T'; 104 static const char tag_2 = 'D'; 105 106 static uint32_t le_device_db_tlv_tag_for_index(uint8_t index){ 107 return (tag_0 << 24) | (tag_1 << 16) | (tag_2 << 8) | index; 108 } 109 110 // @returns success 111 // @param index = entry_pos 112 static int le_device_db_tlv_fetch(int index, le_device_db_entry_t * entry){ 113 if (!le_device_db_tlv_btstack_tlv_impl) return 0; 114 if (index < 0 || index >= NVM_NUM_DEVICE_DB_ENTRIES){ 115 log_error("le_device_db_tlv_fetch called with invalid index %d", index); 116 return 0; 117 } 118 uint32_t tag = le_device_db_tlv_tag_for_index(index); 119 int size = le_device_db_tlv_btstack_tlv_impl->get_tag(le_device_db_tlv_btstack_tlv_context, tag, (uint8_t*) entry, sizeof(le_device_db_entry_t)); 120 return size == sizeof(le_device_db_entry_t); 121 } 122 123 // @returns success 124 // @param index = entry_pos 125 static int le_device_db_tlv_store(int index, le_device_db_entry_t * entry){ 126 if (!le_device_db_tlv_btstack_tlv_impl) return 0; 127 if (index < 0 || index >= NVM_NUM_DEVICE_DB_ENTRIES){ 128 log_error("le_device_db_tlv_store called with invalid index %d", index); 129 return 0; 130 } 131 uint32_t tag = le_device_db_tlv_tag_for_index(index); 132 le_device_db_tlv_btstack_tlv_impl->store_tag(le_device_db_tlv_btstack_tlv_context, tag, (uint8_t*) entry, sizeof(le_device_db_entry_t)); 133 return 1; 134 } 135 136 // @param index = entry_pos 137 static int le_device_db_tlv_delete(int index){ 138 if (!le_device_db_tlv_btstack_tlv_impl) return 0; 139 if (index < 0 || index >= NVM_NUM_DEVICE_DB_ENTRIES){ 140 log_error("le_device_db_tlv_delete called with invalid index %d", index); 141 return 0; 142 } 143 uint32_t tag = le_device_db_tlv_tag_for_index(index); 144 le_device_db_tlv_btstack_tlv_impl->delete_tag(le_device_db_tlv_btstack_tlv_context, tag); 145 return 1; 146 } 147 148 static void le_device_db_tlv_scan(void){ 149 int i; 150 num_valid_entries = 0; 151 memset(entry_map, 0, sizeof(entry_map)); 152 for (i=0;i<NVM_NUM_DEVICE_DB_ENTRIES;i++){ 153 // lookup entry 154 le_device_db_entry_t entry; 155 if (!le_device_db_tlv_fetch(i, &entry)) continue; 156 157 entry_map[i] = 1; 158 num_valid_entries++; 159 } 160 log_info("num valid le device entries %u", num_valid_entries); 161 } 162 163 void le_device_db_init(void){ 164 if (!le_device_db_tlv_btstack_tlv_impl) { 165 log_error("btstack_tlv not initialized"); 166 } 167 } 168 169 // not used 170 void le_device_db_set_local_bd_addr(bd_addr_t bd_addr){ 171 (void)bd_addr; 172 } 173 174 // @returns number of device in db 175 int le_device_db_count(void){ 176 return num_valid_entries; 177 } 178 179 int le_device_db_max_count(void){ 180 return NVM_NUM_DEVICE_DB_ENTRIES; 181 } 182 183 void le_device_db_remove(int index){ 184 // check if entry exists 185 if (entry_map[index] == 0) return; 186 187 // delete entry in TLV 188 le_device_db_tlv_delete(index); 189 190 // mark as unused 191 entry_map[index] = 0; 192 193 // keep track 194 num_valid_entries--; 195 } 196 197 int le_device_db_add(int addr_type, bd_addr_t addr, sm_key_t irk){ 198 199 uint32_t highest_seq_nr = 0; 200 uint32_t lowest_seq_nr = 0xFFFFFFFF; 201 int index_for_lowest_seq_nr = -1; 202 int index_for_addr = -1; 203 int index_for_empty = -1; 204 205 // find unused entry in the used list 206 int i; 207 for (i=0;i<NVM_NUM_DEVICE_DB_ENTRIES;i++){ 208 if (entry_map[i]) { 209 le_device_db_entry_t entry; 210 le_device_db_tlv_fetch(i, &entry); 211 // found addr? 212 if ((memcmp(addr, entry.addr, 6) == 0) && addr_type == entry.addr_type){ 213 index_for_addr = i; 214 } 215 // update highest seq nr 216 if (entry.seq_nr > highest_seq_nr){ 217 highest_seq_nr = entry.seq_nr; 218 } 219 // find entry with lowest seq nr 220 if ((index_for_lowest_seq_nr == -1) || (entry.seq_nr < lowest_seq_nr)){ 221 index_for_lowest_seq_nr = i; 222 lowest_seq_nr = entry.seq_nr; 223 } 224 } else { 225 index_for_empty = i; 226 } 227 } 228 229 log_info("index_for_addr %x, index_for_empy %x, index_for_lowest_seq_nr %x", index_for_addr, index_for_empty, index_for_lowest_seq_nr); 230 231 uint32_t index_to_use = 0; 232 if (index_for_addr >= 0){ 233 index_to_use = index_for_addr; 234 } else if (index_for_empty >= 0){ 235 index_to_use = index_for_empty; 236 } else if (index_for_lowest_seq_nr >= 0){ 237 index_to_use = index_for_lowest_seq_nr; 238 } else { 239 // should not happen 240 return -1; 241 } 242 243 log_info("new entry for index %u", index_to_use); 244 245 // store entry at index 246 le_device_db_entry_t entry; 247 log_info("LE Device DB adding type %u - %s", addr_type, bd_addr_to_str(addr)); 248 log_info_key("irk", irk); 249 250 memset(&entry, 0, sizeof(le_device_db_entry_t)); 251 252 entry.addr_type = addr_type; 253 memcpy(entry.addr, addr, 6); 254 memcpy(entry.irk, irk, 16); 255 entry.seq_nr = highest_seq_nr + 1; 256 #ifdef ENABLE_LE_SIGNED_WRITE 257 entry.remote_counter = 0; 258 #endif 259 260 // store 261 le_device_db_tlv_store(index_to_use, &entry); 262 263 // set in entry_mape 264 entry_map[index_to_use] = 1; 265 266 // keep track - don't increase if old entry found 267 if (index_for_addr < 0){ 268 num_valid_entries++; 269 } 270 271 return index_to_use; 272 } 273 274 275 // get device information: addr type and address 276 void le_device_db_info(int index, int * addr_type, bd_addr_t addr, sm_key_t irk){ 277 278 // fetch entry 279 le_device_db_entry_t entry; 280 int ok = le_device_db_tlv_fetch(index, &entry); 281 if (!ok) return; 282 283 if (addr_type) *addr_type = entry.addr_type; 284 if (addr) memcpy(addr, entry.addr, 6); 285 if (irk) memcpy(irk, entry.irk, 16); 286 } 287 288 void le_device_db_encryption_set(int index, uint16_t ediv, uint8_t rand[8], sm_key_t ltk, int key_size, int authenticated, int authorized){ 289 290 // fetch entry 291 le_device_db_entry_t entry; 292 int ok = le_device_db_tlv_fetch(index, &entry); 293 if (!ok) return; 294 295 // update 296 log_info("LE Device DB set encryption for %u, ediv x%04x, key size %u, authenticated %u, authorized %u", 297 index, ediv, key_size, authenticated, authorized); 298 entry.ediv = ediv; 299 if (rand) memcpy(entry.rand, rand, 8); 300 if (ltk) memcpy(entry.ltk, ltk, 16); 301 entry.key_size = key_size; 302 entry.authenticated = authenticated; 303 entry.authorized = authorized; 304 305 // store 306 le_device_db_tlv_store(index, &entry); 307 } 308 309 void le_device_db_encryption_get(int index, uint16_t * ediv, uint8_t rand[8], sm_key_t ltk, int * key_size, int * authenticated, int * authorized){ 310 311 // fetch entry 312 le_device_db_entry_t entry; 313 int ok = le_device_db_tlv_fetch(index, &entry); 314 if (!ok) return; 315 316 // update user fields 317 log_info("LE Device DB encryption for %u, ediv x%04x, keysize %u, authenticated %u, authorized %u", 318 index, entry.ediv, entry.key_size, entry.authenticated, entry.authorized); 319 if (ediv) *ediv = entry.ediv; 320 if (rand) memcpy(rand, entry.rand, 8); 321 if (ltk) memcpy(ltk, entry.ltk, 16); 322 if (key_size) *key_size = entry.key_size; 323 if (authenticated) *authenticated = entry.authenticated; 324 if (authorized) *authorized = entry.authorized; 325 } 326 327 #ifdef ENABLE_LE_SIGNED_WRITE 328 329 // get signature key 330 void le_device_db_remote_csrk_get(int index, sm_key_t csrk){ 331 332 // fetch entry 333 le_device_db_entry_t entry; 334 int ok = le_device_db_tlv_fetch(index, &entry); 335 if (!ok) return; 336 337 if (csrk) memcpy(csrk, entry.remote_csrk, 16); 338 } 339 340 void le_device_db_remote_csrk_set(int index, sm_key_t csrk){ 341 342 // fetch entry 343 le_device_db_entry_t entry; 344 int ok = le_device_db_tlv_fetch(index, &entry); 345 if (!ok) return; 346 347 if (!csrk) return; 348 349 // update 350 memcpy(entry.remote_csrk, csrk, 16); 351 352 // store 353 le_device_db_tlv_store(index, &entry); 354 } 355 356 void le_device_db_local_csrk_get(int index, sm_key_t csrk){ 357 358 // fetch entry 359 le_device_db_entry_t entry; 360 int ok = le_device_db_tlv_fetch(index, &entry); 361 if (!ok) return; 362 363 if (!csrk) return; 364 365 // fill 366 memcpy(csrk, entry.local_csrk, 16); 367 } 368 369 void le_device_db_local_csrk_set(int index, sm_key_t csrk){ 370 371 // fetch entry 372 le_device_db_entry_t entry; 373 int ok = le_device_db_tlv_fetch(index, &entry); 374 if (!ok) return; 375 376 if (!csrk) return; 377 378 // update 379 memcpy(entry.local_csrk, csrk, 16); 380 381 // store 382 le_device_db_tlv_store(index, &entry); 383 } 384 385 // query last used/seen signing counter 386 uint32_t le_device_db_remote_counter_get(int index){ 387 388 // fetch entry 389 le_device_db_entry_t entry; 390 int ok = le_device_db_tlv_fetch(index, &entry); 391 if (!ok) return 0; 392 393 return entry.remote_counter; 394 } 395 396 // update signing counter 397 void le_device_db_remote_counter_set(int index, uint32_t counter){ 398 399 // fetch entry 400 le_device_db_entry_t entry; 401 int ok = le_device_db_tlv_fetch(index, &entry); 402 if (!ok) return; 403 404 entry.remote_counter = counter; 405 406 // store 407 le_device_db_tlv_store(index, &entry); 408 } 409 410 // query last used/seen signing counter 411 uint32_t le_device_db_local_counter_get(int index){ 412 413 // fetch entry 414 le_device_db_entry_t entry; 415 int ok = le_device_db_tlv_fetch(index, &entry); 416 if (!ok) return 0; 417 418 return entry.local_counter; 419 } 420 421 // update signing counter 422 void le_device_db_local_counter_set(int index, uint32_t counter){ 423 424 // fetch entry 425 le_device_db_entry_t entry; 426 int ok = le_device_db_tlv_fetch(index, &entry); 427 if (!ok) return; 428 429 // update 430 entry.local_counter = counter; 431 432 // store 433 le_device_db_tlv_store(index, &entry); 434 } 435 436 #endif 437 438 void le_device_db_dump(void){ 439 log_info("LE Device DB dump, devices: %d", le_device_db_count()); 440 uint32_t i; 441 442 for (i=0;i<NVM_NUM_DEVICE_DB_ENTRIES;i++){ 443 if (!entry_map[i]) continue; 444 // fetch entry 445 le_device_db_entry_t entry; 446 le_device_db_tlv_fetch(i, &entry); 447 log_info("%u: %u %s", i, entry.addr_type, bd_addr_to_str(entry.addr)); 448 log_info_key("irk", entry.irk); 449 #ifdef ENABLE_LE_SIGNED_WRITE 450 log_info_key("local csrk", entry.local_csrk); 451 log_info_key("remote csrk", entry.remote_csrk); 452 #endif 453 } 454 } 455 456 void le_device_db_tlv_configure(const btstack_tlv_t * btstack_tlv_impl, void * btstack_tlv_context){ 457 le_device_db_tlv_btstack_tlv_impl = btstack_tlv_impl; 458 le_device_db_tlv_btstack_tlv_context = btstack_tlv_context; 459 le_device_db_tlv_scan(); 460 } 461