1 files changed, 306 insertions, 74 deletions

diff --git a/common/partitions/dos.c b/common/partitions/dos.c
index 488c2936f7..8e4edd885b 100644
--- a/common/partitions/dos.c
+++ b/common/partitions/dos.c
@@ -15,12 +15,26 @@
 #include <common.h>
 #include <disks.h>
 #include <init.h>
+#include <stdlib.h>
 #include <asm/unaligned.h>
-#include <dma.h>
 #include <linux/err.h>
+#include <partitions.h>
 
-#include "parser.h"
+struct dos_partition_desc {
+	struct partition_desc pd;
+	uint32_t signature;
+};
 
+struct dos_partition {
+	struct partition part;
+	bool extended;
+	bool logical;
+
+	uint8_t boot_indicator;
+	uint8_t chs_begin[3];
+	uint8_t type;
+	uint8_t chs_end[3];
+};
 
 enum {
 /* These three have identical behaviour; use the second one if DOS FDISK gets
@@ -37,34 +51,9 @@ static inline int is_extended_partition(struct partition *p)
 		p->dos_partition_type == LINUX_EXTENDED_PARTITION);
 }
 
-/**
- * Guess the size of the disk, based on the partition table entries
- * @param dev device to create partitions for
- * @param table partition table
- * @return sector count
- */
-static uint64_t disk_guess_size(struct device_d *dev,
-		struct partition_entry *table)
-{
-	uint64_t size = 0;
-	int i;
-
-	for (i = 0; i < 4; i++) {
-		if (get_unaligned_le32(&table[i].partition_start) != 0) {
-			uint64_t part_end = get_unaligned_le32(&table[i].partition_start) +
-				get_unaligned_le32(&table[i].partition_size);
-
-			if (size < part_end)
-				size = part_end;
-		}
-	}
-
-	return size;
-}
-
 static void *read_mbr(struct block_device *blk)
 {
-	void *buf = dma_alloc(SECTOR_SIZE);
+	void *buf = malloc(SECTOR_SIZE);
 	int ret;
 
 	ret = block_read(blk, buf, 0, 1);
@@ -132,17 +121,18 @@ static int dos_get_disk_signature(struct param_d *p, void *_priv)
 	return 0;
 }
 
-static void dos_extended_partition(struct block_device *blk, struct partition_desc *pd,
+static void dos_extended_partition(struct block_device *blk, struct dos_partition_desc *dpd,
 		struct partition *partition, uint32_t signature)
 {
-	uint8_t *buf = dma_alloc(SECTOR_SIZE);
+	uint8_t *buf = malloc(SECTOR_SIZE);
 	uint32_t ebr_sector = partition->first_sec;
 	struct partition_entry *table = (struct partition_entry *)&buf[0x1be];
-	unsigned partno = 5;
+	unsigned partno = 4;
+	struct dos_partition *dpart;
+	struct partition *pentry;
 
-	while (pd->used_entries < ARRAY_SIZE(pd->parts)) {
+	while (1) {
 		int rc, i;
-		int n = pd->used_entries;
 
 		dev_dbg(blk->dev, "expect EBR in sector %x\n", ebr_sector);
 
@@ -165,15 +155,25 @@ static void dos_extended_partition(struct block_device *blk, struct partition_de
 			}
 		/* /sanity checks */
 
+		dpart = xzalloc(sizeof(*dpart));
+		dpart->logical = true;
+		dpart->boot_indicator = table[0].boot_indicator;
+		memcpy(dpart->chs_begin, table[0].chs_begin, sizeof(table[0].chs_begin));
+		dpart->type = table[0].type;
+		memcpy(dpart->chs_end, table[0].chs_end, sizeof(table[0].chs_end));
+
+		pentry = &dpart->part;
+
 		/* the first entry defines the extended partition */
-		pd->parts[n].first_sec = ebr_sector +
+		pentry->first_sec = ebr_sector +
 			get_unaligned_le32(&table[0].partition_start);
-		pd->parts[n].size = get_unaligned_le32(&table[0].partition_size);
-		pd->parts[n].dos_partition_type = table[0].type;
-		if (signature)
-			sprintf(pd->parts[n].partuuid, "%08x-%02u",
-				signature, partno);
-		pd->used_entries++;
+		pentry->size = get_unaligned_le32(&table[0].partition_size);
+		pentry->dos_partition_type = table[0].type;
+		pentry->num = partno;
+		sprintf(pentry->partuuid, "%08x-%02u", signature, partno + 1);
+
+		list_add_tail(&pentry->list, &dpd->pd.partitions);
+
 		partno++;
 
 		/* the second entry defines the start of the next ebr if != 0 */
@@ -185,10 +185,19 @@ static void dos_extended_partition(struct block_device *blk, struct partition_de
 	}
 
 out:
-	dma_free(buf);
+	free(buf);
 	return;
 }
 
+static void extract_flags(const struct partition_entry *p,
+			  struct partition *pentry)
+{
+	if (p->boot_indicator == 0x80)
+		pentry->flags |= DEVFS_PARTITION_BOOTABLE_LEGACY;
+	if (p->type == 0xef)
+		pentry->flags |= DEVFS_PARTITION_BOOTABLE_ESP;
+}
+
 /**
  * Check if a DOS like partition describes this block device
  * @param blk Block device to register to
@@ -197,57 +206,72 @@ out:
  * It seems at least on ARM this routine cannot use temp. stack space for the
  * sector. So, keep the malloc/free.
  */
-static void dos_partition(void *buf, struct block_device *blk,
-			  struct partition_desc *pd)
+static struct partition_desc *dos_partition(void *buf, struct block_device *blk)
 {
 	struct partition_entry *table;
-	struct partition pentry;
+	struct dos_partition *dpart;
+	struct partition *pentry;
 	struct partition *extended_partition = NULL;
 	uint8_t *buffer = buf;
 	int i;
 	struct disk_signature_priv *dsp;
 	uint32_t signature = get_unaligned_le32(buf + 0x1b8);
+	struct dos_partition_desc *dpd;
+
+	sprintf(blk->cdev.diskuuid, "%08x", signature);
+
+	blk->cdev.flags |= DEVFS_IS_MBR_PARTITIONED;
 
 	table = (struct partition_entry *)&buffer[446];
 
-	/* valid for x86 BIOS based disks only */
-	if (IS_ENABLED(CONFIG_DISK_BIOS) && blk->num_blocks == 0)
-		blk->num_blocks = disk_guess_size(blk->dev, table);
+	dpd = xzalloc(sizeof(*dpd));
+	partition_desc_init(&dpd->pd, blk);
 
 	for (i = 0; i < 4; i++) {
-		pentry.first_sec = get_unaligned_le32(&table[i].partition_start);
-		pentry.size = get_unaligned_le32(&table[i].partition_size);
-		pentry.dos_partition_type = table[i].type;
-
-		if (pentry.first_sec != 0) {
-			int n = pd->used_entries;
-			pd->parts[n].first_sec = pentry.first_sec;
-			pd->parts[n].size = pentry.size;
-			pd->parts[n].dos_partition_type = pentry.dos_partition_type;
-			if (signature)
-				sprintf(pd->parts[n].partuuid, "%08x-%02d",
-						signature, i + 1);
-			pd->used_entries++;
-
-			if (is_extended_partition(&pentry)) {
-				if (!extended_partition)
-					extended_partition = &pd->parts[n];
-				else
-					/*
-					 * An DOS MBR must only contain a single
-					 * extended partition. Just ignore all
-					 * but the first.
-					 */
-					dev_warn(blk->dev, "Skipping additional extended partition\n");
-			}
+		uint64_t first_sec = get_unaligned_le32(&table[i].partition_start);
 
-		} else {
+		if (first_sec == 0) {
 			dev_dbg(blk->dev, "Skipping empty partition %d\n", i);
+			continue;
+		}
+
+		dpart = xzalloc(sizeof(*dpart));
+		dpart->boot_indicator = table[i].boot_indicator;
+		memcpy(dpart->chs_begin, table[i].chs_begin, sizeof(table[i].chs_begin));
+		dpart->type = table[i].type;
+		memcpy(dpart->chs_end, table[i].chs_end, sizeof(table[i].chs_end));
+
+		pentry = &dpart->part;
+
+		pentry->first_sec = first_sec;
+		pentry->size = get_unaligned_le32(&table[i].partition_size);
+		pentry->dos_partition_type = table[i].type;
+		extract_flags(&table[i], pentry);
+		pentry->num = i;
+
+		sprintf(pentry->partuuid, "%08x-%02d", signature, i + 1);
+		dpd->signature = signature;
+
+		if (is_extended_partition(pentry)) {
+			dpart->extended = true;
+			pentry->size = 2;
+
+			if (!extended_partition)
+				extended_partition = pentry;
+			else
+				/*
+				 * An DOS MBR must only contain a single
+				 * extended partition. Just ignore all
+				 * but the first.
+				 */
+				dev_warn(blk->dev, "Skipping additional extended partition\n");
 		}
+
+		list_add_tail(&pentry->list, &dpd->pd.partitions);
 	}
 
 	if (extended_partition)
-		dos_extended_partition(blk, pd, extended_partition, signature);
+		dos_extended_partition(blk, dpd, extended_partition, signature);
 
 	dsp = xzalloc(sizeof(*dsp));
 	dsp->blk = blk;
@@ -265,11 +289,219 @@ static void dos_partition(void *buf, struct block_device *blk,
 	dev_add_param_uint32(blk->dev, "nt_signature",
 			dos_set_disk_signature, dos_get_disk_signature,
 			&dsp->signature, "%08x", dsp);
+
+	return &dpd->pd;
+}
+
+static void dos_partition_free(struct partition_desc *pd)
+{
+	struct partition *part, *tmp;
+
+	list_for_each_entry_safe(part, tmp, &pd->partitions, list) {
+		struct dos_partition *dpart = container_of(part, struct dos_partition, part);
+
+		free(dpart);
+	}
+
+	free(pd);
+}
+
+static __maybe_unused struct partition_desc *dos_partition_create_table(struct block_device *blk)
+{
+	struct dos_partition_desc *dpd = xzalloc(512);
+
+	partition_desc_init(&dpd->pd, blk);
+
+	dpd->signature = random32();
+
+	return &dpd->pd;
+}
+
+static int fs_type_to_type(const char *fstype)
+{
+	unsigned long type;
+	int ret;
+
+	if (!strcmp(fstype, "ext2"))
+		return 0x83;
+	if (!strcmp(fstype, "ext3"))
+		return 0x83;
+	if (!strcmp(fstype, "ext4"))
+		return 0x83;
+	if (!strcmp(fstype, "fat16"))
+		return 0xe;
+	if (!strcmp(fstype, "fat32"))
+		return 0xc;
+
+	ret = kstrtoul(fstype, 16, &type);
+	if (ret)
+		return ret;
+
+	if (type > 0xff)
+		return -EINVAL;
+
+	return type;
+}
+
+static __maybe_unused int dos_partition_mkpart(struct partition_desc *pd,
+					       const char *name, const char *fs_type,
+					       uint64_t start_lba, uint64_t end_lba)
+{
+	struct dos_partition *dpart;
+	struct partition *part, *part_extended = NULL;
+	int npart = 0, npart_logical = 0;
+	uint64_t size;
+	int mask_free = 0xf;
+	int type = fs_type_to_type(fs_type);
+
+	if (type < 0) {
+		pr_err("invalid fs_type \"%s\"\n", fs_type);
+		return -EINVAL;
+	}
+
+	if (start_lba < 1) {
+		pr_err("invalid start LBA, minimum is 1\n");
+		return -EINVAL;
+	}
+
+	list_for_each_entry(part, &pd->partitions, list) {
+		dpart = container_of(part, struct dos_partition, part);
+
+		mask_free &= ~(1 << npart);
+
+		if (dpart->extended)
+			part_extended = part;
+		if (dpart->logical)
+			npart_logical++;
+		else
+			npart++;
+	}
+
+	if (!strcmp(name, "extended")) {
+		if (part_extended) {
+			pr_err("extended partition already exists\n");
+			return -ENOSPC;
+		}
+		goto create;
+	} else if (!strcmp(name, "primary")) {
+		if (npart == 4) {
+			pr_err("Can't create any more partitions\n");
+			return -ENOSPC;
+		}
+
+		goto create;
+	} else if (!strcmp(name, "logical")) {
+		if (!part_extended) {
+			pr_err("No extended partition\n");
+			return -EINVAL;
+		}
+
+		if (npart_logical >= 4) {
+			pr_err("Can't create any more partitions\n");
+			return -ENOSPC;
+		}
+
+		pr_err("Can't create logical partitions yet\n");
+		return -EINVAL;
+	} else {
+		pr_err("Invalid name \"%s\"\n", name);
+		return -EINVAL;
+	}
+
+	return 0;
+
+create:
+	dpart = xzalloc(sizeof(*dpart));
+	part = &dpart->part;
+
+	if (start_lba > UINT_MAX)
+		return -ENOSPC;
+	size = end_lba - start_lba + 1;
+
+	if (size > UINT_MAX)
+		return -ENOSPC;
+
+	dpart->type = fs_type_to_type(fs_type);
+
+	part->first_sec = start_lba;
+	part->size = size;
+	part->num = ffs(mask_free);
+
+	list_add_tail(&part->list, &pd->partitions);
+
+	return 0;
+}
+
+static __maybe_unused int dos_partition_rmpart(struct partition_desc *pd, struct partition *part)
+{
+	struct dos_partition *dpart = container_of(part, struct dos_partition, part);
+
+	list_del(&part->list);
+	free(dpart);
+
+	return 0;
+}
+
+static __maybe_unused int dos_partition_write(struct partition_desc *pd)
+{
+	struct dos_partition_desc *dpd = container_of(pd, struct dos_partition_desc, pd);
+	struct dos_partition *dpart;
+	struct partition *part;
+	void *buf;
+	struct partition_entry *table, *entry;
+	int ret;
+
+	list_for_each_entry(part, &pd->partitions, list) {
+		dpart = container_of(part, struct dos_partition, part);
+		if (dpart->logical) {
+			pr_err("Cannot write tables with logical partitions yet\n");
+			return -EINVAL;
+		}
+	}
+
+	buf = read_mbr(pd->blk);
+	if (!buf)
+		return -EIO;
+
+	put_unaligned_le32(dpd->signature, buf + 0x1b8);
+
+	table = buf + 0x1be;
+	memset(table, 0x0, sizeof(*table) * 4);
+
+	*(u8 *)(buf + 0x1fe) = 0x55;
+	*(u8 *)(buf + 0x1ff) = 0xaa;
+
+	list_for_each_entry(part, &pd->partitions, list) {
+		dpart = container_of(part, struct dos_partition, part);
+
+		entry = &table[part->num - 1];
+
+		entry->boot_indicator = dpart->boot_indicator;
+		memcpy(entry->chs_begin, dpart->chs_begin, sizeof(entry->chs_begin));
+		entry->type = dpart->type;
+		memcpy(entry->chs_end, dpart->chs_end, sizeof(entry->chs_end));
+		put_unaligned_le32(part->first_sec, &entry->partition_start);
+		put_unaligned_le32(part->size, &entry->partition_size);
+	}
+
+	ret = block_write(pd->blk, buf, 0, 1);
+
+	free(buf);
+
+	return ret;
 }
 
 static struct partition_parser dos = {
 	.parse = dos_partition,
+	.partition_free = dos_partition_free,
+#ifdef CONFIG_PARTITION_MANIPULATION
+	.create = dos_partition_create_table,
+	.mkpart = dos_partition_mkpart,
+	.rmpart = dos_partition_rmpart,
+	.write = dos_partition_write,
+#endif
 	.type = filetype_mbr,
+	.name = "msdos",
 };
 
 static int dos_partition_init(void)