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