From 8de6ef19d913d89f9cde8914e324b1f4a41f86e9 Mon Sep 17 00:00:00 2001 From: Marek Szyprowski Date: Thu, 26 Nov 2020 15:25:01 +0100 Subject: [PATCH] part: dos: add code for creating MBR partition layout Add code for creating and writing MBR partition layout. The code generates similar layout of EBRs (Exteneded Block Records) and logical volumes as Linux's fdisk utility. Signed-off-by: Marek Szyprowski Change-Id: I52724de45f32cdde43d900e586d30831906f9280 --- disk/part_dos.c | 167 ++++++++++++++++++++++++++++++++++++++++++++++++ disk/part_dos.h | 2 + include/part.h | 5 ++ 3 files changed, 174 insertions(+) diff --git a/disk/part_dos.c b/disk/part_dos.c index 2c4ad0b6ba..f77f927995 100644 --- a/disk/part_dos.c +++ b/disk/part_dos.c @@ -319,6 +319,173 @@ int is_valid_dos_buf(void *buf) return test_block_type(buf) == DOS_MBR ? 0 : -1; } +#if CONFIG_IS_ENABLED(CMD_MBR) +static void lba_to_chs(lbaint_t lba, unsigned char *rc, unsigned char *rh, + unsigned char *rs) +{ + unsigned int c, h, s; + /* use fixed CHS geometry */ + unsigned int sectpertrack = 63; + unsigned int heads = 255; + + c = (lba + 1) / sectpertrack / heads; + h = (lba + 1) / sectpertrack - c * heads; + s = (lba + 1) - (c * heads + h) * sectpertrack; + + if (c > 1023) { + c = 1023; + h = 254; + s = 63; + } + + *rc = c & 0xff; + *rh = h; + *rs = s + ((c & 0x300) >> 2); +} + +static void mbr_fill_pt_entry(dos_partition_t *pt, lbaint_t start, + lbaint_t relative, lbaint_t size, uchar sys_ind, bool bootable) +{ + pt->boot_ind = bootable ? 0x80 : 0x00; + pt->sys_ind = sys_ind; + lba_to_chs(start, &pt->cyl, &pt->head, &pt->sector); + lba_to_chs(start + size - 1, &pt->end_cyl, &pt->end_head, &pt->end_sector); + put_unaligned_le32(relative, &pt->start4); + put_unaligned_le32(size, &pt->size4); +} + +int write_mbr_partitions(struct blk_desc *dev, + struct disk_partition *p, int count, unsigned int disksig) +{ + ALLOC_CACHE_ALIGN_BUFFER(unsigned char, buffer, dev->blksz); + lbaint_t ext_part_start = 0, ext_part_size = 0, ext_part_sect = 0; + dos_partition_t *pt; + int i; + + memset(buffer, 0, dev->blksz); + buffer[DOS_PART_MAGIC_OFFSET] = 0x55; + buffer[DOS_PART_MAGIC_OFFSET + 1] = 0xaa; + put_unaligned_le32(disksig, &buffer[DOS_PART_DISKSIG_OFFSET]); + pt = (dos_partition_t *) (buffer + DOS_PART_TBL_OFFSET); + + /* create all primary partitions */ + for (i = 0; i < 4 && i < count; i++, pt++) { + mbr_fill_pt_entry(pt, p[i].start, p[i].start, p[i].size, + p[i].sys_ind, p[i].bootable); + if (is_extended(p[i].sys_ind)) { + ext_part_start = p[i].start; + ext_part_size = p[i].size; + ext_part_sect = p[i].start; + } + } + + if (i < count && !ext_part_start) { + printf("%s: extended partition is needed for more than 4 partitions\n", + __func__); + return -1; + } + + /* write MBR */ + if (blk_dwrite(dev, 0, 1, buffer) != 1) { + printf("%s: failed writing 'MBR' (1 blks at 0x0)\n", + __func__); + return -1; + } + + /* create extended volumes */ + for (; i < count; i++) { + lbaint_t next_ebr = 0; + + memset(buffer, 0, dev->blksz); + buffer[DOS_PART_MAGIC_OFFSET] = 0x55; + buffer[DOS_PART_MAGIC_OFFSET + 1] = 0xaa; + pt = (dos_partition_t *) (buffer + DOS_PART_TBL_OFFSET); + + mbr_fill_pt_entry(pt, p[i].start, p[i].start - ext_part_sect, + p[i].size, p[i].sys_ind, p[i].bootable); + + if (i + 1 < count) { + pt++; + next_ebr = p[i].start + p[i].size; + mbr_fill_pt_entry(pt, next_ebr, + next_ebr - ext_part_start, + p[i+1].start + p[i+1].size - next_ebr, + DOS_PART_TYPE_EXTENDED, 0); + } + + /* write EBR */ + if (blk_dwrite(dev, ext_part_sect, 1, buffer) != 1) { + printf("%s: failed writing 'EBR' (1 blks at 0x%lx)\n", + __func__, ext_part_sect); + return -1; + } + ext_part_sect = next_ebr; + } + + return 0; +} + +int layout_mbr_partitions(struct disk_partition *p, int count, + lbaint_t total_sectors) +{ + struct disk_partition *ext = NULL; + int i, j; + lbaint_t ext_vol_start; + + /* calculate primary partitions start and size if needed */ + if (!p[0].start) + p[0].start = DOS_PART_DEFAULT_GAP; + for (i = 0; i < 4 && i < count; i++) { + if (!p[i].start) + p[i].start = p[i - 1].start + p[i - 1].size; + if (!p[i].size) { + lbaint_t end = total_sectors; + lbaint_t allocated = 0; + + for (j = i + 1; j < 4 && j < count; j++) { + if (p[j].start) { + end = p[j].start; + break; + } + allocated += p[j].size; + } + p[i].size = end - allocated - p[i].start; + } + if (p[i].sys_ind == 0x05) + ext = &p[i]; + } + + if (i >= 4 && !ext) { + printf("%s: extended partition is needed for more than 4 partitions\n", + __func__); + return -1; + } + + /* calculate extended volumes start and size if needed */ + ext_vol_start = ext->start; + for (i = 4; i < count; i++) { + if (!p[i].start) + p[i].start = ext_vol_start + DOS_PART_DEFAULT_GAP; + if (!p[i].size) { + lbaint_t end = ext->start + ext->size; + lbaint_t allocated = 0; + + for (j = i + 1; j < count; j++) { + if (p[j].start) { + end = p[j].start - DOS_PART_DEFAULT_GAP; + break; + } + allocated += p[j].size + DOS_PART_DEFAULT_GAP; + } + p[i].size = end - allocated - p[i].start; + } + ext_vol_start = p[i].start + p[i].size; + } + + return 0; +} +#endif + int write_mbr_sector(struct blk_desc *dev_desc, void *buf) { if (is_valid_dos_buf(buf)) diff --git a/disk/part_dos.h b/disk/part_dos.h index dd909a9317..5055822422 100644 --- a/disk/part_dos.h +++ b/disk/part_dos.h @@ -19,6 +19,8 @@ #define DOS_PART_TYPE_EXTENDED_LBA 0x0F #define DOS_PART_TYPE_EXTENDED_LINUX 0x85 +#define DOS_PART_DEFAULT_GAP 2048 + typedef struct dos_partition { unsigned char boot_ind; /* 0x80 - active */ unsigned char head; /* starting head */ diff --git a/include/part.h b/include/part.h index 67b8b2a5cc..fac36364bd 100644 --- a/include/part.h +++ b/include/part.h @@ -474,6 +474,11 @@ int is_valid_dos_buf(void *buf); */ int write_mbr_sector(struct blk_desc *dev_desc, void *buf); +int write_mbr_partitions(struct blk_desc *dev, + struct disk_partition *p, int count, unsigned int disksig); +int layout_mbr_partitions(struct disk_partition *p, int count, + lbaint_t total_sectors); + #endif -- 2.34.1