[PATCH] Split cfi_write_block()
Sebastian Huber
sebastian.huber at embedded-brains.de
Tue Jun 9 13:44:33 UTC 2015
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
More information about the freebsd-hackers
mailing list