Skip site navigation (1)Skip section navigation (2)
Date:      Tue,  9 Jun 2015 15:44:26 +0200
From:      Sebastian Huber <sebastian.huber@embedded-brains.de>
To:        freebsd-hackers@freebsd.org
Cc:        brooks@FreeBSD.org, Sebastian Huber <sebastian.huber@embedded-brains.de>
Subject:   [PATCH] Split cfi_write_block()
Message-ID:  <1433857466-22005-1-git-send-email-sebastian.huber@embedded-brains.de>

next in thread | raw e-mail | index | archive | help
Split cfi_write_block() function into smaller functions to ease reuse of
this code in other systems.  In addition this makes the high level logic
of cfi_write_block() more visible.
---
 dev/cfi/cfi_core.c | 198 ++++++++++++++++++++++++++++++-----------------------
 1 file changed, 114 insertions(+), 84 deletions(-)

diff --git a/dev/cfi/cfi_core.c b/dev/cfi/cfi_core.c
index 934f4cc..da1b018 100644
--- a/dev/cfi/cfi_core.c
+++ b/dev/cfi/cfi_core.c
@@ -634,120 +634,133 @@ cfi_wait_ready(struct cfi_softc *sc, u_int ofs, sbintime_t start,
 	return (error);
 }
 
-int
-cfi_write_block(struct cfi_softc *sc)
+static void
+cfi_unlock_flash(struct cfi_softc *sc, u_int ofs)
 {
-	union {
-		uint8_t		*x8;
-		uint16_t	*x16;
-		uint32_t	*x32;
-	} ptr, cpyprt;
-	register_t intr;
-	int error, i, neederase = 0;
-	uint32_t st;
-	u_int wlen;
-	sbintime_t start;
 
 	/* Intel flash must be unlocked before modification */
 	switch (sc->sc_cmdset) {
 	case CFI_VEND_INTEL_ECS:
 	case CFI_VEND_INTEL_SCS:
-		cfi_write(sc, sc->sc_wrofs, CFI_INTEL_LBS);
-		cfi_write(sc, sc->sc_wrofs, CFI_INTEL_UB);
-		cfi_write(sc, sc->sc_wrofs, CFI_BCS_READ_ARRAY);
+		cfi_write(sc, ofs, CFI_INTEL_LBS);
+		cfi_write(sc, ofs, CFI_INTEL_UB);
+		cfi_write(sc, ofs, CFI_BCS_READ_ARRAY);
 		break;
 	}
+}
 
-	/* Check if an erase is required. */
-	for (i = 0; i < sc->sc_wrbufsz; i++)
-		if ((sc->sc_wrbuf[i] & sc->sc_wrbufcpy[i]) != sc->sc_wrbuf[i]) {
-			neederase = 1;
-			break;
-		}
+static void
+cfi_relock_flash(struct cfi_softc *sc, u_int ofs)
+{
 
-	if (neederase) {
-		intr = intr_disable();
-		start = sbinuptime();
-		/* Erase the block. */
-		switch (sc->sc_cmdset) {
-		case CFI_VEND_INTEL_ECS:
-		case CFI_VEND_INTEL_SCS:
-			cfi_write(sc, sc->sc_wrofs, CFI_BCS_BLOCK_ERASE);
-			cfi_write(sc, sc->sc_wrofs, CFI_BCS_CONFIRM);
-			break;
-		case CFI_VEND_AMD_SCS:
-		case CFI_VEND_AMD_ECS:
-			cfi_amd_write(sc, sc->sc_wrofs, AMD_ADDR_START,
-			    CFI_AMD_ERASE_SECTOR);
-			cfi_amd_write(sc, sc->sc_wrofs, 0, CFI_AMD_BLOCK_ERASE);
-			break;
-		default:
-			/* Better safe than sorry... */
-			intr_restore(intr);
-			return (ENODEV);
-		}
+	/* Relock Intel flash */
+	switch (sc->sc_cmdset) {
+	case CFI_VEND_INTEL_ECS:
+	case CFI_VEND_INTEL_SCS:
+		cfi_write(sc, ofs, CFI_INTEL_LBS);
+		cfi_write(sc, ofs, CFI_INTEL_LB);
+		cfi_write(sc, ofs, CFI_BCS_READ_ARRAY);
+		break;
+	}
+}
+
+static int
+cfi_erase_block(struct cfi_softc *sc, u_int ofs)
+{
+	register_t intr;
+	sbintime_t start;
+
+	intr = intr_disable();
+	start = sbinuptime();
+	/* Erase the block. */
+	switch (sc->sc_cmdset) {
+	case CFI_VEND_INTEL_ECS:
+	case CFI_VEND_INTEL_SCS:
+		cfi_write(sc, ofs, CFI_BCS_BLOCK_ERASE);
+		cfi_write(sc, ofs, CFI_BCS_CONFIRM);
+		break;
+	case CFI_VEND_AMD_SCS:
+	case CFI_VEND_AMD_ECS:
+		cfi_amd_write(sc, ofs, AMD_ADDR_START,
+		    CFI_AMD_ERASE_SECTOR);
+		cfi_amd_write(sc, ofs, 0, CFI_AMD_BLOCK_ERASE);
+		break;
+	default:
+		/* Better safe than sorry... */
 		intr_restore(intr);
-		error = cfi_wait_ready(sc, sc->sc_wrofs, start, 
-		    CFI_TIMEOUT_ERASE);
-		if (error)
-			goto out;
-	} else
-		error = 0;
+		return (ENODEV);
+	}
+	intr_restore(intr);
+	return (cfi_wait_ready(sc, ofs, start, CFI_TIMEOUT_ERASE));
+}
+
+static int
+cfi_program_block(struct cfi_softc *sc, u_int ofs, u_char *wrbuf,
+    u_char *wrbufcpy, u_int wrbufsz, int neederase)
+{
+	union {
+		uint8_t		*x8;
+		uint16_t	*x16;
+		uint32_t	*x32;
+	} ptr, cpyprt;
+	register_t intr;
+	int error = 0;
+	uint32_t st;
+	u_int i, wlen;
+	sbintime_t start;
 
 	/* Write the block using a multibyte write if supported. */
-	ptr.x8 = sc->sc_wrbuf;
-	cpyprt.x8 = sc->sc_wrbufcpy;
+	ptr.x8 = wrbuf;
+	cpyprt.x8 = wrbufcpy;
 	if (sc->sc_maxbuf > sc->sc_width) {
 		switch (sc->sc_cmdset) {
 		case CFI_VEND_INTEL_ECS:
 		case CFI_VEND_INTEL_SCS:
-			for (i = 0; i < sc->sc_wrbufsz; i += wlen) {
-				wlen = MIN(sc->sc_maxbuf, sc->sc_wrbufsz - i);
+			for (i = 0; i < wrbufsz; i += wlen) {
+				wlen = MIN(sc->sc_maxbuf, wrbufsz - i);
 
 				intr = intr_disable();
 
 				start = sbinuptime();
 				do {
-					cfi_write(sc, sc->sc_wrofs + i,
+					cfi_write(sc, ofs + i,
 					    CFI_BCS_BUF_PROG_SETUP);
 					if (sbinuptime() > start + sc->sc_max_timeouts[CFI_TIMEOUT_BUFWRITE]) {
-						error = ETIMEDOUT;
-						goto out;
+						return (ETIMEDOUT);
 					}
-					st = cfi_read(sc, sc->sc_wrofs + i);
+					st = cfi_read(sc, ofs + i);
 				} while (! (st & CFI_INTEL_STATUS_WSMS));
 
-				cfi_write(sc, sc->sc_wrofs + i,
+				cfi_write(sc, ofs + i,
 				    (wlen / sc->sc_width) - 1);
 				switch (sc->sc_width) {
 				case 1:
 					bus_space_write_region_1(sc->sc_tag,
-					    sc->sc_handle, sc->sc_wrofs + i,
+					    sc->sc_handle, ofs + i,
 					    ptr.x8 + i, wlen);
 					break;
 				case 2:
 					bus_space_write_region_2(sc->sc_tag,
-					    sc->sc_handle, sc->sc_wrofs + i,
+					    sc->sc_handle, ofs + i,
 					    ptr.x16 + i / 2, wlen / 2);
 					break;
 				case 4:
 					bus_space_write_region_4(sc->sc_tag,
-					    sc->sc_handle, sc->sc_wrofs + i,
+					    sc->sc_handle, ofs + i,
 					    ptr.x32 + i / 4, wlen / 4);
 					break;
 				}
 
-				cfi_write(sc, sc->sc_wrofs + i,
-				    CFI_BCS_CONFIRM);
+				cfi_write(sc, ofs + i, CFI_BCS_CONFIRM);
 
 				intr_restore(intr);
 
-				error = cfi_wait_ready(sc, sc->sc_wrofs + i,
-				    start, CFI_TIMEOUT_BUFWRITE);
+				error = cfi_wait_ready(sc, ofs + i, start,
+				    CFI_TIMEOUT_BUFWRITE);
 				if (error != 0)
-					goto out;
+					return (error);
 			}
-			goto out;
+			return (error);
 		default:
 			/* Fall through to single word case */
 			break;
@@ -756,7 +769,7 @@ cfi_write_block(struct cfi_softc *sc)
 	}
 
 	/* Write the block one byte/word at a time. */
-	for (i = 0; i < sc->sc_wrbufsz; i += sc->sc_width) {
+	for (i = 0; i < wrbufsz; i += sc->sc_width) {
 
 		/* Avoid writing unless we are actually changing bits */
 		if (!neederase) {
@@ -787,7 +800,7 @@ cfi_write_block(struct cfi_softc *sc)
 		switch (sc->sc_cmdset) {
 		case CFI_VEND_INTEL_ECS:
 		case CFI_VEND_INTEL_SCS:
-			cfi_write(sc, sc->sc_wrofs + i, CFI_BCS_PROGRAM);
+			cfi_write(sc, ofs + i, CFI_BCS_PROGRAM);
 			break;
 		case CFI_VEND_AMD_SCS:
 		case CFI_VEND_AMD_ECS:
@@ -797,40 +810,57 @@ cfi_write_block(struct cfi_softc *sc)
 		switch (sc->sc_width) {
 		case 1:
 			bus_space_write_1(sc->sc_tag, sc->sc_handle,
-			    sc->sc_wrofs + i, *(ptr.x8 + i));
+			    ofs + i, *(ptr.x8 + i));
 			break;
 		case 2:
 			bus_space_write_2(sc->sc_tag, sc->sc_handle,
-			    sc->sc_wrofs + i, *(ptr.x16 + i / 2));
+			    ofs + i, *(ptr.x16 + i / 2));
 			break;
 		case 4:
 			bus_space_write_4(sc->sc_tag, sc->sc_handle,
-			    sc->sc_wrofs + i, *(ptr.x32 + i / 4));
+			    ofs + i, *(ptr.x32 + i / 4));
 			break;
 		}
-		
+
 		intr_restore(intr);
 
-		error = cfi_wait_ready(sc, sc->sc_wrofs, start,
-		   CFI_TIMEOUT_WRITE);
+		error = cfi_wait_ready(sc, ofs, start, CFI_TIMEOUT_WRITE);
 		if (error)
-			goto out;
+			return (error);
 	}
 
 	/* error is 0. */
+	return (error);
+}
+
+int
+cfi_write_block(struct cfi_softc *sc)
+{
+	int error, neederase = 0;
+	u_int i;
+
+	cfi_unlock_flash(sc, sc->sc_wrofs);
+
+	/* Check if an erase is required. */
+	for (i = 0; i < sc->sc_wrbufsz; i++)
+		if ((sc->sc_wrbuf[i] & sc->sc_wrbufcpy[i]) != sc->sc_wrbuf[i]) {
+			neederase = 1;
+			break;
+		}
+
+	if (neederase) {
+		error = cfi_erase_block(sc, sc->sc_wrofs);
+		if (error)
+			goto out;
+	}
+
+	error =  cfi_program_block(sc, sc->sc_wrofs, sc->sc_wrbuf,
+	    sc->sc_wrbufcpy, sc->sc_wrbufsz, neederase);
 
  out:
 	cfi_write(sc, 0, CFI_BCS_READ_ARRAY);
 
-	/* Relock Intel flash */
-	switch (sc->sc_cmdset) {
-	case CFI_VEND_INTEL_ECS:
-	case CFI_VEND_INTEL_SCS:
-		cfi_write(sc, sc->sc_wrofs, CFI_INTEL_LBS);
-		cfi_write(sc, sc->sc_wrofs, CFI_INTEL_LB);
-		cfi_write(sc, sc->sc_wrofs, CFI_BCS_READ_ARRAY);
-		break;
-	}
+	cfi_relock_flash(sc, sc->sc_wrofs);
 	return (error);
 }
 
-- 
1.8.4.5




Want to link to this message? Use this URL: <https://mail-archive.FreeBSD.org/cgi/mid.cgi?1433857466-22005-1-git-send-email-sebastian.huber>