diff options
| -rw-r--r-- | tools/kwboot.c | 35 |
1 files changed, 34 insertions, 1 deletions
diff --git a/tools/kwboot.c b/tools/kwboot.c index 348a3203d60..6bef4610ff8 100644 --- a/tools/kwboot.c +++ b/tools/kwboot.c @@ -1991,6 +1991,39 @@ _inject_baudrate_change_code(void *img, size_t *size, int for_data, } } +static int +kwboot_img_guess_sata_blksz(void *img, uint32_t blkoff, uint32_t data_size, size_t total_size) +{ + uint32_t sum, *ptr, *end; + int blksz; + + /* + * Try all possible sector sizes which are power of two, + * at least 512 bytes and up to the 32 kB. + */ + for (blksz = 512; blksz < 0x10000; blksz *= 2) { + if (blkoff * blksz > total_size || + blkoff * blksz + data_size > total_size || + data_size % 4) + break; + + /* + * Calculate data checksum and if it matches + * then tried blksz should be correct. + */ + ptr = img + blkoff * blksz; + end = (void *)ptr + data_size - 4; + for (sum = 0; ptr < end; ptr++) + sum += *ptr; + + if (sum == *end) + return blksz; + } + + /* Fallback to 512 bytes */ + return 512; +} + static const char * kwboot_img_type(uint8_t blockid) { @@ -2049,7 +2082,7 @@ kwboot_img_patch(void *img, size_t *size, int baudrate) switch (hdr->blockid) { case IBR_HDR_SATA_ID: - hdr->srcaddr = cpu_to_le32(srcaddr * 512); + hdr->srcaddr = cpu_to_le32(srcaddr * kwboot_img_guess_sata_blksz(img, srcaddr, le32_to_cpu(hdr->blocksize), *size)); break; case IBR_HDR_PEX_ID: |