[U-Boot] [PATCH 5/6] Support environment anywhere within erase area

[Guennadi Liakhovetski]

This will become more important with NAND support, in which case the minimum
erase region is a block, which consists of several pages and can be 256KiB
large.

Signed-off-by: Guennadi Liakhovetski <lg at denx.de>
---
 tools/env/fw_env.c |  125 +++++++++++++++++++++++++++++++--------------------
 1 files changed, 76 insertions(+), 49 deletions(-)

diff --git a/tools/env/fw_env.c b/tools/env/fw_env.c
index 556aa85..931e647 100644
--- a/tools/env/fw_env.c
+++ b/tools/env/fw_env.c
@@ -413,11 +413,37 @@ int fw_setenv (int argc, char *argv[])
 	return 0;
 }
 
+static int flash_read_buf (int dev, int fd, void *buf, size_t count,
+			   off_t offset)
+{
+	int rc;
+
+	rc = lseek (fd, offset, SEEK_SET);
+	if (rc == -1) {
+		fprintf (stderr,
+			 "seek error on %s: %s\n",
+			 DEVNAME (dev), strerror (errno));
+		return rc;
+	}
+
+	rc = read (fd, buf, count);
+	if (rc != count) {
+		fprintf (stderr,
+			 "Read error on %s: %s\n",
+			 DEVNAME (dev), strerror (errno));
+		return -1;
+	}
+
+	return rc;
+}
+
 static int flash_write (void)
 {
-	int fd_current, fd_target, rc, dev_target, resid;
+	int fd_current, fd_target, rc, dev_target;
 	erase_info_t erase_current = {}, erase_target;
 	char *data = NULL;
+	off_t erase_offset;
+	struct mtd_info_user mtdinfo_target;
 
 	/* dev_current: fd_current, erase_current */
 	if ((fd_current = open (DEVNAME (dev_current), O_RDWR)) < 0) {
@@ -442,6 +468,45 @@ static int flash_write (void)
 		dev_target = dev_current;
 		fd_target = fd_current;
 	}
+
+	/*
+	 * Support environment anywhere within erase sectors: read out the
+	 * complete area to be erased, replace the environment image, write
+	 * the whole block back again.
+	 */
+	if (DEVESIZE (dev_target) > CFG_ENV_SIZE) {
+		data = malloc (DEVESIZE (dev_target));
+		if (!data) {
+			fprintf (stderr,
+				 "Cannot malloc %lu bytes: %s\n",
+				 DEVESIZE (dev_target),
+				 strerror (errno));
+			return -1;
+		}
+
+		rc = ioctl (fd_target, MEMGETINFO, &mtdinfo_target);
+		if (rc < 0) {
+			perror ("Cannot get MTD information");
+			return -1;
+		}
+
+		/* Erase sector size is always a power of 2 */
+		erase_offset = DEVOFFSET (dev_target) &
+			~(mtdinfo_target.erasesize - 1);
+
+		rc = flash_read_buf (dev_target, fd_target, data,
+				     DEVESIZE (dev_target), erase_offset);
+		if (rc < 0)
+			return rc;
+
+		/* Overwrite the old environment */
+		memcpy(DEVOFFSET (dev_target) - erase_offset + data,
+		       environment.image, CFG_ENV_SIZE);
+	} else {
+		data = (char *)environment.image;
+		erase_offset = DEVOFFSET (dev_target);
+	}
+
 	printf ("Unlocking flash...\n");
 	erase_target.length = DEVESIZE (dev_target);
 	erase_target.start = DEVOFFSET (dev_target);
@@ -455,30 +520,6 @@ static int flash_write (void)
 	}
 
 	printf ("Done\n");
-	resid = DEVESIZE (dev_target) - CFG_ENV_SIZE;
-	if (resid) {
-		if ((data = malloc (resid)) == NULL) {
-			fprintf (stderr,
-				 "Cannot malloc %d bytes: %s\n",
-				 resid,
-				 strerror (errno));
-			return -1;
-		}
-		if (lseek (fd_target, DEVOFFSET (dev_target) + CFG_ENV_SIZE,
-			   SEEK_SET) == -1) {
-			fprintf (stderr, "seek error on %s: %s\n",
-				 DEVNAME (dev_target),
-				 strerror (errno));
-			return -1;
-		}
-		if ((rc = read (fd_target, data, resid)) != resid) {
-			fprintf (stderr,
-				 "read error on %s: %s\n",
-				 DEVNAME (dev_target),
-				 strerror (errno));
-			return -1;
-		}
-	}
 
 	printf ("Erasing old environment...\n");
 
@@ -492,30 +533,24 @@ static int flash_write (void)
 	printf ("Done\n");
 
 	printf ("Writing environment to %s...\n", DEVNAME (dev_target));
-	if (lseek (fd_target, DEVOFFSET (dev_target), SEEK_SET) == -1) {
+	if (lseek (fd_target, erase_offset, SEEK_SET) == -1) {
 		fprintf (stderr,
 			 "seek error on %s: %s\n",
 			 DEVNAME (dev_target), strerror (errno));
 		return -1;
 	}
 
-	if (write (fd_target, environment.image, CFG_ENV_SIZE) !=
-	    CFG_ENV_SIZE) {
+	if (write (fd_target, data, DEVESIZE (dev_target)) !=
+	    DEVESIZE (dev_target)) {
 		fprintf (stderr,
 			 "Write error on %s: %s\n",
 			 DEVNAME (dev_target), strerror (errno));
 		return -1;
 	}
 
-	if (resid) {
-		if (write (fd_target, data, resid) != resid) {
-			fprintf (stderr,
-				 "write error on %s: %s\n",
-				 DEVNAME (dev_current), strerror (errno));
-			return -1;
-		}
+	if (DEVESIZE (dev_target) > CFG_ENV_SIZE)
 		free (data);
-	}
+
 	if (HaveRedundEnv) {
 		/* change flag on current active env partition */
 		if (lseek (fd_current, DEVOFFSET (dev_current) + sizeof (ulong),
@@ -560,7 +595,7 @@ static int flash_write (void)
 
 static int flash_read (void)
 {
-	int fd;
+	int fd, rc;
 
 	if ((fd = open (DEVNAME (dev_current), O_RDONLY)) < 0) {
 		fprintf (stderr,
@@ -569,18 +604,10 @@ static int flash_read (void)
 		return -1;
 	}
 
-	if (lseek (fd, DEVOFFSET (dev_current), SEEK_SET) == -1) {
-		fprintf (stderr,
-			 "seek error on %s: %s\n",
-			 DEVNAME (dev_current), strerror (errno));
-		return -1;
-	}
-	if (read (fd, environment.image, CFG_ENV_SIZE) != CFG_ENV_SIZE) {
-		fprintf (stderr,
-			 "Read error on %s: %s\n",
-			 DEVNAME (dev_current), strerror (errno));
-		return -1;
-	}
+	rc = flash_read_buf (dev_current, fd, environment.image, CFG_ENV_SIZE,
+			     DEVOFFSET (dev_current));
+	if (rc < 0)
+		return rc;
 
 	if (close (fd)) {
 		fprintf (stderr,
-- 
1.5.4