diff --git a/drivers/mtd/parsers/afs.c b/drivers/mtd/parsers/afs.c
index 72c688b8a383b27dd0338c8fe9d0c465f8117b6d..0c730024f806cdae565e0ebc966e84e2d0873c87 100644
--- a/drivers/mtd/parsers/afs.c
+++ b/drivers/mtd/parsers/afs.c
@@ -3,6 +3,7 @@
drivers/mtd/afs.c: ARM Flash Layout/Partitioning
Copyright © 2000 ARM Limited
+ Copyright (C) 2019 Linus Walleij
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
@@ -35,6 +36,8 @@
#include <linux/mtd/partitions.h>
#define AFSV1_FOOTER_MAGIC 0xA0FFFF9F
+#define AFSV2_FOOTER_MAGIC1 0x464C5348 /* "FLSH" */
+#define AFSV2_FOOTER_MAGIC2 0x464F4F54 /* "FOOT" */
struct footer_v1 {
u32 image_info_base; /* Address of first word of ImageFooter */
@@ -68,6 +71,22 @@ static u32 word_sum(void *words, int num)
return sum;
}
+static u32 word_sum_v2(u32 *p, u32 num)
+{
+ u32 sum = 0;
+ int i;
+
+ for (i = 0; i < num; i++) {
+ u32 val;
+
+ val = p[i];
+ if (val > ~sum)
+ sum++;
+ sum += val;
+ }
+ return ~sum;
+}
+
static bool afs_is_v1(struct mtd_info *mtd, u_int off)
{
/* The magic is 12 bytes from the end of the erase block */
@@ -88,6 +107,27 @@ static bool afs_is_v1(struct mtd_info *mtd, u_int off)
return (magic == AFSV1_FOOTER_MAGIC);
}
+static bool afs_is_v2(struct mtd_info *mtd, u_int off)
+{
+ /* The magic is the 8 last bytes of the erase block */
+ u_int ptr = off + mtd->erasesize - 8;
+ u32 foot[2];
+ size_t sz;
+ int ret;
+
+ ret = mtd_read(mtd, ptr, 8, &sz, (u_char *)foot);
+ if (ret < 0) {
+ printk(KERN_ERR "AFS: mtd read failed at 0x%x: %d\n",
+ ptr, ret);
+ return false;
+ }
+ if (ret >= 0 && sz != 8)
+ return false;
+
+ return (foot[0] == AFSV2_FOOTER_MAGIC1 &&
+ foot[1] == AFSV2_FOOTER_MAGIC2);
+}
+
static int afs_parse_v1_partition(struct mtd_info *mtd,
u_int off, struct mtd_partition *part)
{
@@ -185,6 +225,113 @@ static int afs_parse_v1_partition(struct mtd_info *mtd,
return 0;
}
+static int afs_parse_v2_partition(struct mtd_info *mtd,
+ u_int off, struct mtd_partition *part)
+{
+ u_int ptr;
+ u32 footer[12];
+ u32 imginfo[36];
+ char *name;
+ u32 version;
+ u32 entrypoint;
+ u32 attributes;
+ u32 region_count;
+ u32 block_start;
+ u32 block_end;
+ u32 crc;
+ size_t sz;
+ int ret;
+ int i;
+ int pad = 0;
+
+ pr_debug("Parsing v2 partition @%08x-%08x\n",
+ off, off + mtd->erasesize);
+
+ /* First read the footer */
+ ptr = off + mtd->erasesize - sizeof(footer);
+ ret = mtd_read(mtd, ptr, sizeof(footer), &sz, (u_char *)footer);
+ if ((ret < 0) || (ret >= 0 && sz != sizeof(footer))) {
+ pr_err("AFS: mtd read failed at 0x%x: %d\n",
+ ptr, ret);
+ return -EIO;
+ }
+ name = (char *) &footer[0];
+ version = footer[9];
+ ptr = off + mtd->erasesize - sizeof(footer) - footer[8];
+
+ pr_debug("found image \"%s\", version %08x, info @%08x\n",
+ name, version, ptr);
+
+ /* Then read the image information */
+ ret = mtd_read(mtd, ptr, sizeof(imginfo), &sz, (u_char *)imginfo);
+ if ((ret < 0) || (ret >= 0 && sz != sizeof(imginfo))) {
+ pr_err("AFS: mtd read failed at 0x%x: %d\n",
+ ptr, ret);
+ return -EIO;
+ }
+
+ /* 32bit platforms have 4 bytes padding */
+ crc = word_sum_v2(&imginfo[1], 34);
+ if (!crc) {
+ pr_debug("Padding 1 word (4 bytes)\n");
+ pad = 1;
+ } else {
+ /* 64bit platforms have 8 bytes padding */
+ crc = word_sum_v2(&imginfo[2], 34);
+ if (!crc) {
+ pr_debug("Padding 2 words (8 bytes)\n");
+ pad = 2;
+ }
+ }
+ if (crc) {
+ pr_err("AFS: bad checksum on v2 image info: %08x\n", crc);
+ return -EINVAL;
+ }
+ entrypoint = imginfo[pad];
+ attributes = imginfo[pad+1];
+ region_count = imginfo[pad+2];
+ block_start = imginfo[20];
+ block_end = imginfo[21];
+
+ pr_debug("image entry=%08x, attr=%08x, regions=%08x, "
+ "bs=%08x, be=%08x\n",
+ entrypoint, attributes, region_count,
+ block_start, block_end);
+
+ for (i = 0; i < region_count; i++) {
+ u32 region_load_addr = imginfo[pad + 3 + i*4];
+ u32 region_size = imginfo[pad + 4 + i*4];
+ u32 region_offset = imginfo[pad + 5 + i*4];
+ u32 region_start;
+ u32 region_end;
+
+ pr_debug(" region %d: address: %08x, size: %08x, "
+ "offset: %08x\n",
+ i,
+ region_load_addr,
+ region_size,
+ region_offset);
+
+ region_start = off + region_offset;
+ region_end = region_start + region_size;
+ /* Align partition to end of erase block */
+ region_end += (mtd->erasesize - 1);
+ region_end &= ~(mtd->erasesize -1);
+ pr_debug(" partition start = %08x, partition end = %08x\n",
+ region_start, region_end);
+
+ /* Create one partition per region */
+ part->name = kstrdup(name, GFP_KERNEL);
+ if (!part->name)
+ return -ENOMEM;
+ part->offset = region_start;
+ part->size = region_end - region_start;
+ part->mask_flags = 0;
+ }
+
+ return 0;
+}
+
static int parse_afs_partitions(struct mtd_info *mtd,
const struct mtd_partition **pparts,
struct mtd_part_parser_data *data)
@@ -200,6 +347,10 @@ static int parse_afs_partitions(struct mtd_info *mtd,
sz += sizeof(struct mtd_partition);
i += 1;
}
+ if (afs_is_v2(mtd, off)) {
+ sz += sizeof(struct mtd_partition);
+ i += 1;
+ }
}
if (!i)
@@ -213,13 +364,18 @@ static int parse_afs_partitions(struct mtd_info *mtd,
* Identify the partitions
*/
for (i = off = 0; off < mtd->size; off += mtd->erasesize) {
-
if (afs_is_v1(mtd, off)) {
ret = afs_parse_v1_partition(mtd, off, &parts[i]);
if (ret)
goto out_free_parts;
i++;
}
+ if (afs_is_v2(mtd, off)) {
+ ret = afs_parse_v2_partition(mtd, off, &parts[i]);
+ if (ret)
+ goto out_free_parts;
+ i++;
+ }
}
*pparts = parts;