|
@@ -19,6 +19,7 @@
|
19
|
19
|
FILE_LICENCE ( GPL2_OR_LATER );
|
20
|
20
|
|
21
|
21
|
#include <stdint.h>
|
|
22
|
+#include <stdlib.h>
|
22
|
23
|
#include <unistd.h>
|
23
|
24
|
#include <errno.h>
|
24
|
25
|
#include <byteswap.h>
|
|
@@ -392,3 +393,163 @@ int pci_vpd_find ( struct pci_vpd *vpd, unsigned int field,
|
392
|
393
|
PCI_ARGS ( vpd->pci ), PCI_VPD_FIELD_ARGS ( field ) );
|
393
|
394
|
return -ENOENT;
|
394
|
395
|
}
|
|
396
|
+
|
|
397
|
+/**
|
|
398
|
+ * Resize VPD field
|
|
399
|
+ *
|
|
400
|
+ * @v vpd PCI VPD
|
|
401
|
+ * @v field VPD field descriptor
|
|
402
|
+ * @v len New length of field body
|
|
403
|
+ * @ret address Address of field body
|
|
404
|
+ * @ret rc Return status code
|
|
405
|
+ */
|
|
406
|
+int pci_vpd_resize ( struct pci_vpd *vpd, unsigned int field, size_t len,
|
|
407
|
+ unsigned int *address ) {
|
|
408
|
+ struct pci_vpd_field rw_field;
|
|
409
|
+ struct pci_vpd_field old_field;
|
|
410
|
+ struct pci_vpd_field new_field;
|
|
411
|
+ unsigned int rw_address;
|
|
412
|
+ unsigned int old_address;
|
|
413
|
+ unsigned int copy_address;
|
|
414
|
+ unsigned int dst_address;
|
|
415
|
+ unsigned int dump_address;
|
|
416
|
+ size_t rw_len;
|
|
417
|
+ size_t old_len;
|
|
418
|
+ size_t available_len;
|
|
419
|
+ size_t copy_len;
|
|
420
|
+ size_t dump_len;
|
|
421
|
+ void *copy;
|
|
422
|
+ int rc;
|
|
423
|
+
|
|
424
|
+ /* Sanity checks */
|
|
425
|
+ assert ( PCI_VPD_TAG ( field ) == PCI_VPD_TAG_RW );
|
|
426
|
+ assert ( PCI_VPD_KEYWORD ( field ) != 0 );
|
|
427
|
+ assert ( field != PCI_VPD_FIELD_RW );
|
|
428
|
+
|
|
429
|
+ /* Locate 'RW' field */
|
|
430
|
+ if ( ( rc = pci_vpd_find ( vpd, PCI_VPD_FIELD_RW, &rw_address,
|
|
431
|
+ &rw_len ) ) != 0 )
|
|
432
|
+ goto err_no_rw;
|
|
433
|
+
|
|
434
|
+ /* Locate old field, if any */
|
|
435
|
+ if ( ( rc = pci_vpd_find ( vpd, field, &old_address,
|
|
436
|
+ &old_len ) ) == 0 ) {
|
|
437
|
+
|
|
438
|
+ /* Field already exists */
|
|
439
|
+ if ( old_address > rw_address ) {
|
|
440
|
+ DBGC ( vpd, PCI_FMT " VPD field " PCI_VPD_FIELD_FMT
|
|
441
|
+ " at [%04x,%04zx) is after field "
|
|
442
|
+ PCI_VPD_FIELD_FMT " at [%04x,%04zx)\n",
|
|
443
|
+ PCI_ARGS ( vpd->pci ),
|
|
444
|
+ PCI_VPD_FIELD_ARGS ( field ),
|
|
445
|
+ old_address, ( old_address + old_len ),
|
|
446
|
+ PCI_VPD_FIELD_ARGS ( PCI_VPD_FIELD_RW ),
|
|
447
|
+ rw_address, ( rw_address + rw_len ) );
|
|
448
|
+ rc = -ENXIO;
|
|
449
|
+ goto err_after_rw;
|
|
450
|
+ }
|
|
451
|
+ dst_address = ( old_address - sizeof ( old_field ) );
|
|
452
|
+ copy_address = ( old_address + old_len );
|
|
453
|
+ copy_len = ( rw_address - sizeof ( rw_field ) - copy_address );
|
|
454
|
+
|
|
455
|
+ /* Calculate available length */
|
|
456
|
+ available_len = ( rw_len + old_len );
|
|
457
|
+
|
|
458
|
+ } else {
|
|
459
|
+
|
|
460
|
+ /* Field does not yet exist */
|
|
461
|
+ dst_address = ( rw_address - sizeof ( rw_field ) );
|
|
462
|
+ copy_address = dst_address;
|
|
463
|
+ copy_len = 0;
|
|
464
|
+
|
|
465
|
+ /* Calculate available length */
|
|
466
|
+ available_len = ( ( rw_len > sizeof ( new_field ) ) ?
|
|
467
|
+ ( rw_len - sizeof ( new_field ) ) : 0 );
|
|
468
|
+ }
|
|
469
|
+
|
|
470
|
+ /* Dump region before changes */
|
|
471
|
+ dump_address = dst_address;
|
|
472
|
+ dump_len = ( rw_address + rw_len - dump_address );
|
|
473
|
+ DBGC ( vpd, PCI_FMT " VPD before resizing field " PCI_VPD_FIELD_FMT
|
|
474
|
+ " to %zd bytes:\n", PCI_ARGS ( vpd->pci ),
|
|
475
|
+ PCI_VPD_FIELD_ARGS ( field ), len );
|
|
476
|
+ pci_vpd_dump ( vpd, dump_address, dump_len );
|
|
477
|
+
|
|
478
|
+ /* Check available length */
|
|
479
|
+ if ( available_len > PCI_VPD_MAX_LEN )
|
|
480
|
+ available_len = PCI_VPD_MAX_LEN;
|
|
481
|
+ if ( len > available_len ) {
|
|
482
|
+ DBGC ( vpd, PCI_FMT " VPD no space for field "
|
|
483
|
+ PCI_VPD_FIELD_FMT " (need %02zx, have %02zx)\n",
|
|
484
|
+ PCI_ARGS ( vpd->pci ), PCI_VPD_FIELD_ARGS ( field ),
|
|
485
|
+ len, available_len );
|
|
486
|
+ rc = -ENOSPC;
|
|
487
|
+ goto err_no_space;
|
|
488
|
+ }
|
|
489
|
+
|
|
490
|
+ /* Preserve intermediate fields, if any */
|
|
491
|
+ copy = malloc ( copy_len );
|
|
492
|
+ if ( ! copy ) {
|
|
493
|
+ rc = -ENOMEM;
|
|
494
|
+ goto err_copy_alloc;
|
|
495
|
+ }
|
|
496
|
+ if ( ( rc = pci_vpd_read ( vpd, copy_address, copy, copy_len ) ) != 0 )
|
|
497
|
+ goto err_copy_read;
|
|
498
|
+
|
|
499
|
+ /* Create new field, if applicable */
|
|
500
|
+ if ( len ) {
|
|
501
|
+ new_field.keyword = PCI_VPD_KEYWORD ( field );
|
|
502
|
+ new_field.len = len;
|
|
503
|
+ if ( ( rc = pci_vpd_write ( vpd, dst_address, &new_field,
|
|
504
|
+ sizeof ( new_field ) ) ) != 0 )
|
|
505
|
+ goto err_new_write;
|
|
506
|
+ dst_address += sizeof ( new_field );
|
|
507
|
+ *address = dst_address;
|
|
508
|
+ DBGC ( vpd, PCI_FMT " VPD field " PCI_VPD_FIELD_FMT " is now "
|
|
509
|
+ "at [%04x,%04x)\n", PCI_ARGS ( vpd->pci ),
|
|
510
|
+ PCI_VPD_FIELD_ARGS ( field ), dst_address,
|
|
511
|
+ ( dst_address + new_field.len ) );
|
|
512
|
+ dst_address += len;
|
|
513
|
+ } else {
|
|
514
|
+ DBGC ( vpd, PCI_FMT " VPD field " PCI_VPD_FIELD_FMT
|
|
515
|
+ " no longer exists\n", PCI_ARGS ( vpd->pci ),
|
|
516
|
+ PCI_VPD_FIELD_ARGS ( field ) );
|
|
517
|
+ }
|
|
518
|
+
|
|
519
|
+ /* Restore intermediate fields, if any */
|
|
520
|
+ if ( ( rc = pci_vpd_write ( vpd, dst_address, copy, copy_len ) ) != 0 )
|
|
521
|
+ goto err_copy_write;
|
|
522
|
+ dst_address += copy_len;
|
|
523
|
+
|
|
524
|
+ /* Create 'RW' field */
|
|
525
|
+ rw_field.keyword = PCI_VPD_KEYWORD ( PCI_VPD_FIELD_RW );
|
|
526
|
+ rw_field.len = ( rw_len +
|
|
527
|
+ ( rw_address - sizeof ( rw_field ) ) - dst_address );
|
|
528
|
+ if ( ( rc = pci_vpd_write ( vpd, dst_address, &rw_field,
|
|
529
|
+ sizeof ( rw_field ) ) ) != 0 )
|
|
530
|
+ goto err_rw_write;
|
|
531
|
+ dst_address += sizeof ( rw_field );
|
|
532
|
+ DBGC ( vpd, PCI_FMT " VPD field " PCI_VPD_FIELD_FMT " is now "
|
|
533
|
+ "at [%04x,%04x)\n", PCI_ARGS ( vpd->pci ),
|
|
534
|
+ PCI_VPD_FIELD_ARGS ( PCI_VPD_FIELD_RW ), dst_address,
|
|
535
|
+ ( dst_address + rw_field.len ) );
|
|
536
|
+
|
|
537
|
+ /* Dump region after changes */
|
|
538
|
+ DBGC ( vpd, PCI_FMT " VPD after resizing field " PCI_VPD_FIELD_FMT
|
|
539
|
+ " to %zd bytes:\n", PCI_ARGS ( vpd->pci ),
|
|
540
|
+ PCI_VPD_FIELD_ARGS ( field ), len );
|
|
541
|
+ pci_vpd_dump ( vpd, dump_address, dump_len );
|
|
542
|
+
|
|
543
|
+ rc = 0;
|
|
544
|
+
|
|
545
|
+ err_rw_write:
|
|
546
|
+ err_new_write:
|
|
547
|
+ err_copy_write:
|
|
548
|
+ err_copy_read:
|
|
549
|
+ free ( copy );
|
|
550
|
+ err_copy_alloc:
|
|
551
|
+ err_no_space:
|
|
552
|
+ err_after_rw:
|
|
553
|
+ err_no_rw:
|
|
554
|
+ return rc;
|
|
555
|
+}
|