1 /* 2 ** Copyright 2012-2013 Intel Corporation 3 ** 4 ** Licensed under the Apache License, Version 2.0 (the "License"); 5 ** you may not use this file except in compliance with the License. 6 ** You may obtain a copy of the License at 7 ** 8 ** http://www.apache.org/licenses/LICENSE-2.0 9 ** 10 ** Unless required by applicable law or agreed to in writing, software 11 ** distributed under the License is distributed on an "AS IS" BASIS, 12 ** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 ** See the License for the specific language governing permissions and 14 ** limitations under the License. 15 */ 16 17 #include <stdbool.h> /* defines bool type */ 18 #include <stddef.h> /* defines size_t */ 19 #include <stdint.h> /* defines integer types with specified widths */ 20 #include <stdio.h> /* defines FILE */ 21 #include <string.h> /* defines memcpy and memset */ 22 23 #include "libtbd.h" /* our own header file */ 24 25 /*! 26 * \brief Debug messages. 27 */ 28 #ifdef __ANDROID__ 29 #define LOG_TAG "libtbd" 30 #include <utils/Log.h> 31 #define MSG_LOG(...) LOGD(__VA_ARGS__) 32 #define MSG_ERR(...) LOGE(__VA_ARGS__) 33 #else 34 #include <stdio.h> 35 #define MSG_LOG(...) fprintf(stdout, __VA_ARGS__); fprintf(stdout, "\n"); 36 #define MSG_ERR(...) fprintf(stderr, __VA_ARGS__); fprintf(stderr, "\n"); 37 #endif 38 39 /* 40 * Checks the validity of the pointer 41 * param[in] a_ptr Pointer to be examined 42 * return True if pointer ok 43 */ 44 bool is_valid_pointer(void* a_ptr) 45 { 46 if ((!a_ptr) || ((unsigned long)(a_ptr) % sizeof(uint32_t))) { 47 return false; 48 } else { 49 return true; 50 } 51 } 52 53 /* 54 * Calculates checksum for a data block. 55 * param[in] a_data_ptr Data from where to calculate the checksum 56 * param[in] a_data_size Size of the data 57 * return The checksum 58 */ 59 uint32_t get_checksum(void *a_data_ptr, size_t a_data_size) 60 { 61 uint32_t *ptr32 = a_data_ptr; 62 int size32 = a_data_size / sizeof(uint32_t); 63 64 /* Simple checksum algorithm: summing up the data content 65 * as 32-bit numbers */ 66 uint32_t checksum32 = 0; 67 if (size32) { 68 if (size32 & 0x01) { 69 checksum32 += *ptr32++; 70 size32 -= 1; 71 } 72 if (size32 & 0x02) { 73 checksum32 += *ptr32++; 74 checksum32 += *ptr32++; 75 size32 -= 2; 76 } 77 for (; size32 > 0; size32 -= 4) { 78 checksum32 += *ptr32++; 79 checksum32 += *ptr32++; 80 checksum32 += *ptr32++; 81 checksum32 += *ptr32++; 82 } 83 } 84 85 return checksum32; 86 } 87 88 /* 89 * Common subroutine to validate Tagged Binary Data container, without 90 * paying attention to checksum or data tagging. This function assumes 91 * that the data resides in "legal" memory area as there is no size 92 * given together with input pointer. 93 * param[in] a_data_ptr Pointer to container 94 * return Return code indicating possible errors 95 */ 96 tbd_error_t validate_anysize(void *a_data_ptr) 97 { 98 uint8_t *byte_ptr, *eof_ptr; 99 tbd_record_header_t *record_ptr; 100 uint32_t record_size; 101 102 /* Container should begin with a header */ 103 tbd_header_t *header_ptr = a_data_ptr; 104 105 /* Check against illegal pointers */ 106 if (!is_valid_pointer(header_ptr)) { 107 MSG_ERR("LIBTBD ERROR: Cannot access data!"); 108 return tbd_err_data; 109 } 110 111 /* Check that the indicated data size makes sense, 112 * and is not too much or too little */ 113 if (header_ptr->size % sizeof(uint32_t)) { 114 MSG_ERR("LIBTBD ERROR: Size in header should be multiple of 4 bytes!"); 115 return tbd_err_data; 116 } 117 if (header_ptr->size < sizeof(tbd_header_t)) { 118 MSG_ERR("LIBTBD ERROR: Invalid data header!"); 119 return tbd_err_data; 120 } 121 122 /* First record is just after header, a byte pointer is needed 123 * to do math with sizes and pointers */ 124 byte_ptr = (uint8_t *)(header_ptr + 1); 125 eof_ptr = (uint8_t *)(a_data_ptr) + header_ptr->size; 126 127 /* Loop until there are no more records to go */ 128 while (byte_ptr < eof_ptr) { 129 /* At least one more record is expected */ 130 131 /* Record header must be within the given data size */ 132 if (byte_ptr + sizeof(tbd_record_header_t) > eof_ptr) { 133 MSG_ERR("LIBTBD ERROR: Invalid data header!"); 134 return tbd_err_data; 135 } 136 137 record_ptr = (tbd_record_header_t *)(byte_ptr); 138 record_size = record_ptr->size; 139 140 /* Check that the indicated record size makes sense, 141 * and is not too much or too little */ 142 if (record_size % sizeof(uint32_t)) { 143 MSG_ERR("LIBTBD ERROR: Size in record should be multiple of 4 bytes!"); 144 return tbd_err_data; 145 } 146 if (record_size < sizeof(tbd_record_header_t)) { 147 MSG_ERR("LIBTBD ERROR: Invalid record header!"); 148 return tbd_err_data; 149 } 150 if (byte_ptr + record_size > eof_ptr) { 151 MSG_ERR("LIBTBD ERROR: Invalid record header!"); 152 return tbd_err_data; 153 } 154 155 /* This record ok, continue the while loop... */ 156 byte_ptr += record_size; 157 } 158 159 /* Seems that we have a valid data with no more headers */ 160 return tbd_err_none; 161 } 162 163 /* 164 * Common subroutine to validate Tagged Binary Data, without paying 165 * attention to checksum or data tagging. Also, this function does 166 * check that the data fits in the given buffer size. 167 * param[in] a_data_ptr Pointer to data buffer 168 * param[in] a_data_size Size of the data buffer 169 * return Return code indicating possible errors 170 */ 171 tbd_error_t validate(void *a_data_ptr, size_t a_data_size) 172 { 173 /* Container should begin with a header */ 174 tbd_header_t *header_ptr = a_data_ptr; 175 176 /* Check against illegal pointers */ 177 if (!is_valid_pointer(header_ptr)) { 178 MSG_ERR("LIBTBD ERROR: Cannot access data!"); 179 return tbd_err_data; 180 } 181 182 /* Check that the TBD header fits into given data */ 183 if (sizeof(tbd_header_t) > a_data_size) { 184 MSG_ERR("TBD ERROR: #1 Too small data buffer given!"); 185 return tbd_err_data; 186 } 187 188 /* Check that the indicated data fits in the buffer */ 189 if (header_ptr->size > a_data_size) { 190 MSG_ERR("TBD ERROR: #2 Too small data buffer given!"); 191 return tbd_err_data; 192 } 193 194 /* Check the the content is ok */ 195 return validate_anysize(a_data_ptr); 196 } 197 198 /* 199 * Creates a new, empty Tagged Binary Data container with the tag 200 * that was given. Also updates the checksum and size accordingly. 201 * Note that the buffer size must be large enough for the header 202 * to fit in, the exact amount being 24 bytes (for tbd_header_t). 203 * param[in] a_data_ptr Pointer to modifiable container buffer 204 * param[in] a_data_size Size of the container buffer 205 * param[in] a_tag Tag the container shall have 206 * param[out] a_new_size Updated container size 207 * return Return code indicating possible errors 208 */ 209 tbd_error_t tbd_create(void *a_data_ptr, size_t a_data_size 210 , tbd_tag_t a_tag, size_t *a_new_size) 211 { 212 tbd_header_t *header_ptr; 213 214 /* Check that the TBD header fits into given data */ 215 if (sizeof(tbd_header_t) > a_data_size) { 216 MSG_ERR("LIBTBD ERROR: Not enough data given!"); 217 return tbd_err_argument; 218 } 219 220 /* Nullify everything */ 221 memset(a_data_ptr, 0, sizeof(tbd_header_t)); 222 223 /* The header is what we need */ 224 header_ptr = a_data_ptr; 225 226 header_ptr->tag = a_tag; 227 228 header_ptr->size = sizeof(tbd_header_t); 229 header_ptr->version = IA_TBD_VERSION; 230 header_ptr->revision = IA_TBD_REVISION; 231 header_ptr->config_bits = 0; 232 header_ptr->checksum = get_checksum(header_ptr, sizeof(tbd_header_t)); 233 234 *a_new_size = sizeof(tbd_header_t); 235 236 return tbd_err_none; 237 } 238 239 /* 240 * Performs number of checks to given Tagged Binary Data container, 241 * including the verification of the checksum. The function does not 242 * care about the tag type of the container. 243 * param[in] a_data_ptr Pointer to container buffer 244 * param[in] a_data_size Size of the container buffer 245 * return Return code indicating possible errors 246 */ 247 tbd_error_t tbd_validate_anytag(void *a_data_ptr, size_t a_data_size) 248 { 249 tbd_header_t *header_ptr; 250 251 /* Check the the content is ok */ 252 int r; 253 if ((r = validate(a_data_ptr, a_data_size))) { 254 return r; 255 } 256 257 /* Container should begin with a header */ 258 header_ptr = a_data_ptr; 259 260 /* Check that the checksum is correct */ 261 262 /* When calculating the checksum for the original data, the checksum 263 * field has been filled with zero value - so after inserting the 264 * checksum in its place, the new calculated checksum is actually 265 * two times the original */ 266 267 if (get_checksum(header_ptr, header_ptr->size) - header_ptr->checksum != header_ptr->checksum) { 268 MSG_ERR("LIBTBD ERROR: Checksum doesn't match!"); 269 return tbd_err_data; 270 } 271 272 /* Seems that we have valid data */ 273 return tbd_err_none; 274 } 275 276 /* 277 * Performs number of checks to given Tagged Binary Data container, 278 * including the verification of the checksum. Also, the data must have 279 * been tagged properly. The tag is further used to check endianness, 280 * and if it seems wrong, a specific debug message is printed out. 281 * param[in] a_data_ptr Pointer to container buffer 282 * param[in] a_data_size Size of the container buffer 283 * param[in] a_tag Tag the data must have 284 * return Return code indicating possible errors 285 */ 286 tbd_error_t tbd_validate(void *a_data_ptr, size_t a_data_size 287 , tbd_tag_t a_tag) 288 { 289 tbd_header_t *header_ptr; 290 291 /* Check the the content is ok */ 292 int r; 293 if ((r = validate(a_data_ptr, a_data_size))) { 294 return r; 295 } 296 297 /* Container should begin with a header */ 298 header_ptr = a_data_ptr; 299 300 /* Check that the tag is correct */ 301 if (header_ptr->tag != a_tag) { 302 /* See if we have wrong endianness or incorrect tag */ 303 uint32_t reverse_tag = ( (((a_tag) >> 24) & 0x000000FF) 304 | (((a_tag) >> 8) & 0x0000FF00) 305 | (((a_tag) << 8) & 0x00FF0000) 306 | (((a_tag) << 24) & 0xFF000000) ); 307 308 if (reverse_tag == header_ptr->tag) { 309 MSG_ERR("LIBTBD ERROR: Wrong endianness of data!"); 310 } else { 311 MSG_ERR("LIBTBD ERROR: Data is not tagged properly!"); 312 } 313 return tbd_err_data; 314 } 315 316 /* Check that the checksum is correct */ 317 318 /* When calculating the checksum for the original data, the checksum 319 * field has been filled with zero value - so after inserting the 320 * checksum in its place, the new calculated checksum is actually 321 * two times the original */ 322 323 if (get_checksum(header_ptr, header_ptr->size) - header_ptr->checksum != header_ptr->checksum) { 324 MSG_ERR("LIBTBD ERROR: Checksum doesn't match!"); 325 return tbd_err_data; 326 } 327 328 /* Seems that we have valid data */ 329 return tbd_err_none; 330 } 331 332 /* 333 * Checks if a given kind of record exists in the Tagged Binary Data, 334 * and if yes, tells the location of such record as well as its size. 335 * If there are multiple records that match the query, the indicated 336 * record is the first one. 337 * param[in] a_data_ptr Pointer to container buffer 338 * param[in] a_record_class Class the record must have 339 * param[in] a_record_format Format the record must have 340 * param[out] a_record_data Record data (or NULL if not found) 341 * param[out] a_record_size Record size (or 0 if not found) 342 * return Return code indicating possible errors 343 */ 344 tbd_error_t tbd_get_record(void *a_data_ptr 345 , tbd_class_t a_record_class, tbd_format_t a_record_format 346 , void **a_record_data, uint32_t *a_record_size) 347 { 348 tbd_header_t *header_ptr; 349 uint8_t *byte_ptr, *eof_ptr; 350 351 /* Check the the content is ok */ 352 int r; 353 if ((r = validate_anysize(a_data_ptr))) { 354 return r; 355 } 356 357 /* Container should begin with a header */ 358 header_ptr = a_data_ptr; 359 360 /* First record is just after header */ 361 byte_ptr = (uint8_t *)(header_ptr + 1); 362 eof_ptr = (uint8_t *)(a_data_ptr) + header_ptr->size; 363 364 /* Loop until there are no more records to go */ 365 while (byte_ptr < eof_ptr) { 366 /* At least one more record is expected */ 367 tbd_record_header_t *record_ptr = (tbd_record_header_t *)(byte_ptr); 368 369 uint16_t record_class = record_ptr->class_id; 370 uint8_t record_format = record_ptr->format_id; 371 uint32_t record_size = record_ptr->size; 372 373 if (((a_record_class == tbd_class_any) || (a_record_class == record_class)) 374 && ((a_record_format == tbd_format_any) || (a_record_format == record_format))) { 375 376 /* Match found */ 377 *a_record_data = record_ptr + 1; 378 *a_record_size = record_size - sizeof(tbd_record_header_t); 379 380 return tbd_err_none; 381 382 } 383 384 /* Match not found yet, continue the while loop... */ 385 byte_ptr += record_size; 386 } 387 388 MSG_LOG("libtbd: Record not found!"); 389 *a_record_data = NULL; 390 *a_record_size = 0; 391 return tbd_err_none; 392 } 393 394 /* 395 * The given record is inserted into the Tagged Binary Data container 396 * that must exist already. New records are always added to the end, 397 * regardless if a record with the same class and format field already 398 * exists in the data. Also updates the checksum and size accordingly. 399 * Note that the buffer size must be large enough for the inserted 400 * record to fit in, the exact amount being the size of original 401 * Tagged Binary Data container plus the size of record data to be 402 * inserted plus 8 bytes (for tbd_record_header_t). 403 * param[in] a_data_ptr Pointer to modifiable container buffer 404 * param[in] a_data_size Size of buffer (surplus included) 405 * param[in] a_record_class Class the record shall have 406 * param[in] a_record_format Format the record shall have 407 * param[in] a_record_data Record data 408 * param[in] a_record_size Record size 409 * param[out] a_new_size Updated container size 410 * return Return code indicating possible errors 411 */ 412 tbd_error_t tbd_insert_record(void *a_data_ptr, size_t a_data_size 413 , tbd_class_t a_record_class, tbd_format_t a_record_format 414 , void *a_record_data, size_t a_record_size 415 , size_t *a_new_size) 416 { 417 tbd_header_t *header_ptr; 418 size_t new_size; 419 tbd_record_header_t *record_ptr; 420 int r; 421 422 /* Check the the content is ok */ 423 if ((r = validate(a_data_ptr, a_data_size))) { 424 return r; 425 } 426 427 /* Container should begin with a header */ 428 header_ptr = a_data_ptr; 429 430 /* Check that the new record fits into given data */ 431 new_size = header_ptr->size + sizeof(tbd_record_header_t) + a_record_size; 432 433 if (new_size > a_data_size) { 434 MSG_ERR("LIBTBD ERROR: #3 Too small data buffer given!"); 435 return tbd_err_argument; 436 } 437 438 /* Check against illegal pointers */ 439 if (!is_valid_pointer(a_record_data)) { 440 MSG_ERR("LIBTBD ERROR: Cannot access data!"); 441 return tbd_err_data; 442 } 443 444 /* Check that the indicated data size makes sense */ 445 if (a_record_size % sizeof(uint32_t)) { 446 MSG_ERR("LIBTBD ERROR: Size in record should be multiple of 4 bytes!"); 447 return tbd_err_data; 448 } 449 450 /* Where our record should go */ 451 record_ptr = (tbd_record_header_t *)((char *)(a_data_ptr) + header_ptr->size); 452 453 /* Create record header and store the record itself */ 454 record_ptr->size = sizeof(tbd_record_header_t) + a_record_size; 455 record_ptr->format_id = a_record_format; 456 record_ptr->packing_key = 0; 457 record_ptr->class_id = a_record_class; 458 record_ptr++; 459 memcpy(record_ptr, a_record_data, a_record_size); 460 461 /* Update the header */ 462 header_ptr->size = new_size; 463 header_ptr->checksum = 0; 464 header_ptr->checksum = get_checksum(header_ptr, new_size); 465 466 *a_new_size = new_size; 467 468 return tbd_err_none; 469 } 470 471 /* 472 * The indicated record is removed from the Tagged Binary Data, after 473 * which the checksum and size are updated accordingly. If there are 474 * multiple records that match the class and format, only the first 475 * instance is removed. If no record is found, nothing will be done. 476 * Note that the resulting Tagged Binary Data container will 477 * be smaller than the original, but it does not harm to store the 478 * resulting container in its original length, either. 479 * param[in] a_data_ptr Pointer to modifiable container buffer 480 * param[in] a_record_class Class the record should have 481 * param[in] a_record_format Format the record should have 482 * param[out] a_new_size Updated container size 483 * return Return code indicating possible errors 484 */ 485 tbd_error_t tbd_remove_record(void *a_data_ptr 486 , tbd_class_t a_record_class, tbd_format_t a_record_format 487 , size_t *a_new_size) 488 { 489 tbd_header_t *header_ptr; 490 uint8_t *byte_ptr, *eof_ptr; 491 size_t new_size; 492 493 /* Check the the content is ok */ 494 int r; 495 if ((r = validate_anysize(a_data_ptr))) { 496 return r; 497 } 498 499 /* Container should begin with a header */ 500 header_ptr = a_data_ptr; 501 502 /* First record is just after header */ 503 byte_ptr = (uint8_t *)(header_ptr + 1); 504 eof_ptr = (uint8_t *)(a_data_ptr) + header_ptr->size; 505 506 /* Loop until there are no more records to go */ 507 while (byte_ptr < eof_ptr) { 508 /* At least one more record is expected */ 509 tbd_record_header_t *record_ptr = (tbd_record_header_t *)(byte_ptr); 510 511 uint16_t record_class = record_ptr->class_id; 512 uint8_t record_format = record_ptr->format_id; 513 uint32_t record_size = record_ptr->size; 514 515 if (((a_record_class == tbd_class_any) || (a_record_class == record_class)) 516 && ((a_record_format == tbd_format_any) || (a_record_format == record_format))) { 517 518 /* Match found, remove the record */ 519 memcpy(byte_ptr, byte_ptr + record_size, eof_ptr - (byte_ptr + record_size)); 520 521 /* Update the header */ 522 new_size = header_ptr->size - record_size; 523 header_ptr->size = new_size; 524 header_ptr->checksum = 0; 525 header_ptr->checksum = get_checksum(header_ptr, new_size); 526 527 *a_new_size = new_size; 528 529 return tbd_err_none; 530 531 } 532 533 /* Match not found yet, continue the while loop... */ 534 byte_ptr += record_size; 535 } 536 537 MSG_LOG("libtbd: Record not found!"); 538 *a_new_size = header_ptr->size; 539 return tbd_err_none; 540 } 541 542 /* 543 * Validates the Tagged Binary data container and generates a human 544 * readable detailed report on the content, including information about 545 * the records contained. 546 * param[in] a_data_ptr Pointer to container buffer 547 * param[in] a_data_size Size of the container buffer 548 * param[in] a_outfile Pointer to open file (may be stdout) 549 * return Return code indicating possible errors 550 */ 551 tbd_error_t tbd_infoprint(void *a_data_ptr, size_t a_data_size 552 , FILE *a_outfile) 553 { 554 tbd_header_t *header_ptr; 555 uint8_t *byte_ptr, *eof_ptr, record_format, record_packing; 556 int num_of_records = 0, total_data = 0; 557 uint16_t record_class; 558 uint32_t record_size; 559 560 /* Check the the content is ok */ 561 int r; 562 if ((r = validate(a_data_ptr, a_data_size))) { 563 return r; 564 } 565 566 /* Container should begin with a header */ 567 header_ptr = a_data_ptr; 568 569 fprintf(a_outfile, "Data tag: 0x%08x (\'%c\' \'%c\' \'%c\' \'%c\')\n", header_ptr->tag, ((char *)(&header_ptr->tag))[0], ((char *)(&header_ptr->tag))[1], ((char *)(&header_ptr->tag))[2], ((char *)(&header_ptr->tag))[3]); 570 fprintf(a_outfile, "Data size: %d (0x%x), buffer size %d (0x%x)\n", header_ptr->size, header_ptr->size, (uint32_t)a_data_size, (uint32_t)a_data_size); 571 fprintf(a_outfile, "Data version: 0x%08x\n", header_ptr->version); 572 fprintf(a_outfile, "Data revision: 0x%08x\n", header_ptr->revision); 573 fprintf(a_outfile, "Data config: 0x%08x\n", header_ptr->config_bits); 574 fprintf(a_outfile, "Data checksum: 0x%08x\n", header_ptr->checksum); 575 576 fprintf(a_outfile, "\n"); 577 578 /* First record is just after header */ 579 byte_ptr = (uint8_t *)(header_ptr + 1); 580 eof_ptr = (uint8_t *)(a_data_ptr) + header_ptr->size; 581 582 /* Loop until there are no more records to go */ 583 while (byte_ptr < eof_ptr) { 584 /* At least one more record is expected */ 585 tbd_record_header_t *record_ptr = (tbd_record_header_t *)(byte_ptr); 586 num_of_records++; 587 588 record_class = record_ptr->class_id; 589 record_format = record_ptr->format_id; 590 record_packing = record_ptr->packing_key; 591 record_size = record_ptr->size; 592 total_data += record_size - sizeof(tbd_record_header_t); 593 594 fprintf(a_outfile, "Record size: %d (0x%x)\n", record_size, record_size); 595 fprintf(a_outfile, "Size w/o header: %d (0x%x)\n", record_size - (uint32_t)sizeof(tbd_record_header_t), record_size - (uint32_t)sizeof(tbd_record_header_t)); 596 fprintf(a_outfile, "Record class: %d", record_class); 597 switch (record_class) { 598 case tbd_class_any: 599 fprintf(a_outfile, " \"tbd_class_any\"\n"); 600 break; 601 case tbd_class_aiq: 602 fprintf(a_outfile, " \"tbd_class_aiq\"\n"); 603 break; 604 case tbd_class_drv: 605 fprintf(a_outfile, " \"tbd_class_drv\"\n"); 606 break; 607 case tbd_class_hal: 608 fprintf(a_outfile, " \"tbd_class_hal\"\n"); 609 break; 610 default: 611 fprintf(a_outfile, " (unknown class)\n"); 612 break; 613 } 614 fprintf(a_outfile, "Record format: %d", record_format); 615 switch (record_format) { 616 case tbd_format_any: 617 fprintf(a_outfile, " \"tbd_format_any\"\n"); 618 break; 619 case tbd_format_custom: 620 fprintf(a_outfile, " \"tbd_format_custom\"\n"); 621 break; 622 case tbd_format_container: 623 fprintf(a_outfile, " \"tbd_format_container\"\n"); 624 break; 625 default: 626 fprintf(a_outfile, " (unknown format)\n"); 627 break; 628 } 629 fprintf(a_outfile, "Packing: %d", record_packing); 630 if (record_packing == 0) { 631 fprintf(a_outfile, " (no packing)\n"); 632 } else { 633 fprintf(a_outfile, "\n"); 634 } 635 636 fprintf(a_outfile, "\n"); 637 638 /* Continue the while loop... */ 639 byte_ptr += record_size; 640 } 641 642 fprintf(a_outfile, "Number of records found: %d\n", num_of_records); 643 fprintf(a_outfile, "Total data in records: %d bytes (without headers)\n", total_data); 644 fprintf(a_outfile, "\n"); 645 return tbd_err_none; 646 } 647 648