3 files changed, 100 insertions, 6 deletions

diff --git a/sys/dev/cfi/cfi_core.c b/sys/dev/cfi/cfi_core.c
index d0888b5881d3..e1470dcfd115 100644
--- a/sys/dev/cfi/cfi_core.c
+++ b/sys/dev/cfi/cfi_core.c
@@ -421,6 +421,16 @@ cfi_attach(device_t dev)
 		}
 	}
 
+	if (sc->sc_cmdset == CFI_VEND_AMD_ECS  ||
+	    sc->sc_cmdset == CFI_VEND_AMD_SCS) {
+		cfi_amd_write(sc, 0, AMD_ADDR_START, CFI_AMD_AUTO_SELECT);
+		sc->sc_manid = cfi_read(sc, 0);
+		sc->sc_devid = cfi_read(sc, 2);
+		device_printf(dev, "Manufacturer ID:%x Device ID:%x\n",
+		    sc->sc_manid, sc->sc_devid);
+		cfi_write(sc, 0, CFI_BCS_READ_ARRAY2);
+	}
+
 	u = device_get_unit(dev);
 	sc->sc_nod = make_dev(&cfi_cdevsw, u, UID_ROOT, GID_WHEEL, 0600,
 	    "%s%u", cfi_driver_name, u);
@@ -500,6 +510,37 @@ cfi_detach(device_t dev)
 	return (0);
 }
 
+static bool
+cfi_check_erase(struct cfi_softc *sc, u_int ofs, u_int sz)
+{
+	bool result;
+	int i;
+	uint32_t val;
+
+	result = FALSE;
+	for (i = 0; i < sz; i += sc->sc_width) {
+		val = cfi_read(sc, ofs + i);
+		switch (sc->sc_width) {
+		case 1:
+			if (val != 0xff)
+				goto out;
+			continue;
+		case 2:
+			if (val != 0xffff)
+				goto out;
+			continue;
+		case 4:
+			if (val != 0xffffffff)
+				goto out;
+			continue;
+		}
+	}
+	result = TRUE;
+
+out:
+	return (result);
+}
+
 static int
 cfi_wait_ready(struct cfi_softc *sc, u_int ofs, sbintime_t start,
     enum cfi_wait_cmd cmd)
@@ -581,10 +622,12 @@ cfi_write_block(struct cfi_softc *sc)
 		uint32_t	*x32;
 	} ptr, cpyprt;
 	register_t intr;
-	int error, i, neederase = 0;
+	int error, i, j, neederase = 0;
 	uint32_t st;
 	u_int wlen;
 	sbintime_t start;
+	u_int minsz;
+	uint32_t val;
 
 	/* Intel flash must be unlocked before modification */
 	switch (sc->sc_cmdset) {
@@ -615,9 +658,27 @@ cfi_write_block(struct cfi_softc *sc)
 			break;
 		case CFI_VEND_AMD_SCS:
 		case CFI_VEND_AMD_ECS:
+			/* find minimum sector size */
+			minsz = sc->sc_region[0].r_blksz;
+			for (i = 1; i < sc->sc_regions; i++) {
+				if (sc->sc_region[i].r_blksz < minsz)
+					minsz = sc->sc_region[i].r_blksz;
+			}
 			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);
+			cfi_amd_write(sc, sc->sc_wrofs, 
+			    sc->sc_wrofs >> (ffs(minsz) - 1),
+			    CFI_AMD_BLOCK_ERASE);
+			for (i = 0; i < CFI_AMD_MAXCHK; ++i) {
+				if (cfi_check_erase(sc, sc->sc_wrofs,
+				    sc->sc_wrbufsz))
+					break;
+				DELAY(10);
+			}
+			if (i == CFI_AMD_MAXCHK) {
+				printf("\nCFI Sector Erase time out error\n");
+				return (ENODEV);
+			}
 			break;
 		default:
 			/* Better safe than sorry... */
@@ -749,10 +810,37 @@ cfi_write_block(struct cfi_softc *sc)
 		
 		intr_restore(intr);
 
-		error = cfi_wait_ready(sc, sc->sc_wrofs, start,
-		   CFI_TIMEOUT_WRITE);
-		if (error)
-			goto out;
+		if (sc->sc_cmdset == CFI_VEND_AMD_ECS  ||
+		    sc->sc_cmdset == CFI_VEND_AMD_SCS) {
+			for (j = 0; j < CFI_AMD_MAXCHK; ++j) {
+				switch (sc->sc_width) {
+				case 1:
+					val = *(ptr.x8 + i);
+					break;
+				case 2:
+					val = *(ptr.x16 + i / 2);
+					break;
+				case 4:
+					val = *(ptr.x32 + i / 4);
+					break;
+				}
+
+				if (cfi_read(sc, sc->sc_wrofs + i) == val)
+					break;
+					
+				DELAY(10);
+			}
+			if (j == CFI_AMD_MAXCHK) {
+				printf("\nCFI Program Verify time out error\n");
+				error = ENXIO;
+				goto out;
+			}
+		} else {
+			error = cfi_wait_ready(sc, sc->sc_wrofs, start,
+			   CFI_TIMEOUT_WRITE);
+			if (error)
+				goto out;
+		}
 	}
 
 	/* error is 0. */
diff --git a/sys/dev/cfi/cfi_reg.h b/sys/dev/cfi/cfi_reg.h
index c2a95c47fe55..53f4b6d4e864 100644
--- a/sys/dev/cfi/cfi_reg.h
+++ b/sys/dev/cfi/cfi_reg.h
@@ -146,10 +146,13 @@ struct cfi_qry {
 #define	CFI_AMD_BLOCK_ERASE	0x30
 #define	CFI_AMD_UNLOCK_ACK	0x55
 #define	CFI_AMD_ERASE_SECTOR	0x80
+#define	CFI_AMD_AUTO_SELECT	0x90
 #define	CFI_AMD_PROGRAM		0xa0
 #define	CFI_AMD_UNLOCK		0xaa
 
 #define	AMD_ADDR_START		0xaaa
 #define	AMD_ADDR_ACK		0x555
 
+#define	CFI_AMD_MAXCHK		0x10000
+
 #endif /* _DEV_CFI_REG_H_ */
diff --git a/sys/dev/cfi/cfi_var.h b/sys/dev/cfi/cfi_var.h
index 8c83782e874c..4ac579c36a10 100644
--- a/sys/dev/cfi/cfi_var.h
+++ b/sys/dev/cfi/cfi_var.h
@@ -80,6 +80,9 @@ struct cfi_softc {
 	u_int		sc_wrbufsz;
 	u_int		sc_wrofs;
 	u_int		sc_writing;
+
+	u_int		sc_manid;
+	u_int		sc_devid;
 };
 
 extern char cfi_driver_name[];