Skip site navigation (1)Skip section navigation (2)
Date:      Mon, 14 Mar 2011 17:09:42 +0200
From:      Aleksandr Rybalko <ray@dlink.ua>
To:        freebsd-current@freebsd.org, freebsd-embedded@freebsd.org
Subject:   [CFT][patch]cfi driver support for NOR flash arrays
Message-ID:  <20110314170942.90bfb5a8.ray@dlink.ua>

next in thread | raw e-mail | index | archive | help
This is a multi-part message in MIME format.

--Multipart=_Mon__14_Mar_2011_17_09_42_+0200_kl6/fo6bUqwAX_PL
Content-Type: text/plain; charset=US-ASCII
Content-Transfer-Encoding: 7bit

Hi, all.

proposed patch add support of NOR flash arrays to cfi driver
http://my.ddteam.net/files/2011-03-11_cfi_flash_array_support.patch

Supported arrays split by address (example: first on offset 0, second on offset 0x400000) and interleaved (example: two devices
with 16bit width, mapped to 32bit word)

Tested on simple cfi flash and on Samsung K8Q2815UQB which have two 8M devices selected by A22 pin.

I will be very glad if someone help me with testing on interleaved or split+interleaved array.

Wait for you questions and recommendations.

P.S. Also driver have CFI_AMD_BYPASS flag which enable AMD type flash to program word with 2 bus access instead of 4 by default.

-- 
Alexandr Rybalko <ray@dlink.ua> 
aka Alex RAY <ray@ddteam.net>

--Multipart=_Mon__14_Mar_2011_17_09_42_+0200_kl6/fo6bUqwAX_PL
Content-Type: text/x-diff;
 name="2011-03-11_cfi_flash_array_support.patch"
Content-Disposition: attachment;
	filename="2011-03-11_cfi_flash_array_support.patch"
Content-Transfer-Encoding: 7bit

Index: sys/dev/cfi/cfi_var.h
===================================================================
--- sys/dev/cfi/cfi_var.h	(revision 219085)
+++ sys/dev/cfi/cfi_var.h	(working copy)
@@ -44,15 +44,28 @@
 	bus_space_handle_t sc_handle;
 	bus_space_tag_t	sc_tag;
 	int		sc_rid;
+	u_long		sc_winsize;	/* Bus window size. */
 
 	u_int		sc_size;	/* Flash size. */
+	u_int		sc_array_width;	/* Chips in array */
+	u_int		sc_rows;	/* Rows count */
+
+	u_int		sc_manid;	/* Manufacturer ID. */
+	u_int		sc_devid;	/* Device Code. */
+
+	u_int		sc_mask;	/* Array mask */
 	u_int		sc_width;	/* Interface width. */
+	u_int		sc_shift;	/* Command address shift */
+
 	u_int		sc_regions;	/* Erase regions. */
 	struct cfi_region *sc_region;	/* Array of region info. */
 
 	u_int		sc_cmdset;
 	u_int		sc_erase_timeout;
+	u_int		sc_erase_max_timeout;
 	u_int		sc_write_timeout;
+	u_int		sc_write_max_timeout;
+	u_int		sc_rstcmd;	/* Read Array command code */
 
 	struct cdev	*sc_nod;
 	struct proc	*sc_opened;	/* Process that has us opened. */
@@ -71,11 +84,11 @@
 int cfi_attach(device_t);
 int cfi_detach(device_t);
 
-uint32_t cfi_read(struct cfi_softc *, u_int);
-uint8_t cfi_read_qry(struct cfi_softc *, u_int);
+u_int cfi_read(struct cfi_softc *, u_int, u_int);
 int cfi_write_block(struct cfi_softc *);
 int cfi_block_start(struct cfi_softc *, u_int);
 int cfi_block_finish(struct cfi_softc *);
+int cfi_erase_block(struct cfi_softc *, u_int);
 
 #ifdef CFI_SUPPORT_STRATAFLASH
 int	cfi_intel_get_factory_pr(struct cfi_softc *sc, uint64_t *);
Index: sys/dev/cfi/cfi_dev.c
===================================================================
--- sys/dev/cfi/cfi_dev.c	(revision 219085)
+++ sys/dev/cfi/cfi_dev.c	(working copy)
@@ -103,7 +103,7 @@
 	/* Read the block from flash for byte-serving. */
 	ptr.x8 = sc->sc_wrbuf;
 	for (r = 0; r < sc->sc_wrbufsz; r += sc->sc_width) {
-		val = cfi_read(sc, sc->sc_wrofs + r);
+		val = cfi_read(sc, sc->sc_wrofs + r, 0);
 		switch (sc->sc_width) {
 		case 1:
 			*(ptr.x8)++ = val;
@@ -188,7 +188,7 @@
 	while (error == 0 && uio->uio_resid > 0 &&
 	    uio->uio_offset < sc->sc_size) {
 		ofs = uio->uio_offset;
-		val = cfi_read(sc, ofs);
+		val = cfi_read(sc, ofs, 0);
 		switch (sc->sc_width) {
 		case 1:
 			buf.x8[0] = val;
@@ -271,7 +271,7 @@
 			return (ENOSPC);
 
 		while (!error && rq->count--) {
-			val = cfi_read_qry(sc, rq->offset++);
+			val = cfi_read(sc, rq->offset++, 0);
 			error = copyout(&val, rq->buffer++, 1);
 		}
 		break;
Index: sys/dev/cfi/cfi_bus_obio.c
===================================================================
--- sys/dev/cfi/cfi_bus_obio.c	(revision 0)
+++ sys/dev/cfi/cfi_bus_obio.c	(revision 0)
@@ -0,0 +1,75 @@
+/*-
+ * Copyright (c) 2007, Juniper Networks, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the author nor the names of any co-contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/conf.h>
+#include <sys/kernel.h>
+#include <sys/malloc.h>   
+#include <sys/module.h>
+#include <sys/rman.h>
+#include <sys/sysctl.h>
+
+#include <machine/bus.h>
+
+#include <dev/cfi/cfi_var.h>
+
+
+static int cfi_obio_probe(device_t);
+
+static device_method_t cfi_obio_methods[] = {
+	/* device interface */
+	DEVMETHOD(device_probe,		cfi_obio_probe),
+	DEVMETHOD(device_attach,	cfi_attach),
+	DEVMETHOD(device_detach,	cfi_detach),
+
+	{0, 0}
+};
+
+static driver_t cfi_obio_driver = {
+	cfi_driver_name,
+	cfi_obio_methods,
+	sizeof(struct cfi_softc),
+};
+
+DRIVER_MODULE (cfi, obio, cfi_obio_driver, cfi_devclass, 0, 0);
+
+static int
+cfi_obio_probe(device_t dev)
+{
+	struct cfi_softc *sc = device_get_softc(dev);
+	sc->sc_width = 1;	/* NB: don't probe interface width */
+	sc->sc_shift = 2;
+
+	return (cfi_probe(dev));
+}

Property changes on: sys/dev/cfi/cfi_bus_obio.c
___________________________________________________________________
Added: svn:mime-type
   + text/plain
Added: svn:keywords
   + FreeBSD=%H
Added: svn:eol-style
   + native

Index: sys/dev/cfi/cfi_reg.h
===================================================================
--- sys/dev/cfi/cfi_reg.h	(revision 219085)
+++ sys/dev/cfi/cfi_reg.h	(working copy)
@@ -35,17 +35,17 @@
 struct cfi_qry {
 	u_char		reserved[16];
 	u_char		ident[3];	/* "QRY" */
-	u_char		pri_vend[2];
+	u_char		pri_vend_cmdset[2];
 	u_char		pri_vend_eqt[2];
-	u_char		alt_vend[2];
+	u_char		alt_vend_cmdset[2];
 	u_char		alt_vend_eqt[2];
 	/* System Interface Information. */
 	u_char		min_vcc;
 	u_char		max_vcc;
 	u_char		min_vpp;
 	u_char		max_vpp;
-	u_char		tto_byte_write;		/* 2**n milliseconds. */
-	u_char		tto_buf_write;		/* 2**n milliseconds. */
+	u_char		tto_byte_write;		/* 2**n microseconds. */
+	u_char		tto_buf_write;		/* 2**n microseconds. */
 	u_char		tto_block_erase;	/* 2**n milliseconds. */
 	u_char		tto_chip_erase;		/* 2**n milliseconds. */
 	u_char		mto_byte_write;		/* 2**n times typical t/o. */
@@ -67,7 +67,10 @@
 #define	CFI_QRY_CMD_DATA	0x98
 
 #define	CFI_QRY_IDENT		offsetof(struct cfi_qry, ident)
-#define	CFI_QRY_VEND		offsetof(struct cfi_qry, pri_vend)
+#define	CFI_QRY_PRI_VEND_CMDSET		offsetof(struct cfi_qry, pri_vend_cmdset)
+#define	CFI_QRY_ALT_VEND_CMDSET		offsetof(struct cfi_qry, alt_vend_cmdset)
+#define	CFI_QRY_PRI_OFFSET		offsetof(struct cfi_qry, pri_vend_eqt)
+#define	CFI_QRY_ALT_OFFSET		offsetof(struct cfi_qry, alt_vend_eqt)
 
 #define	CFI_QRY_TTO_WRITE	offsetof(struct cfi_qry, tto_byte_write)
 #define	CFI_QRY_TTO_ERASE	offsetof(struct cfi_qry, tto_block_erase)
@@ -130,10 +133,15 @@
 #define	CFI_AMD_BLOCK_ERASE	0x30
 #define	CFI_AMD_UNLOCK_ACK	0x55
 #define	CFI_AMD_ERASE_SECTOR	0x80
+#define	CFI_AMD_GET_ID		0x90
+#define		CFI_AMD_MANID_ADDR		0x00
+#define		CFI_AMD_DEVID_ADDR		0x01
 #define	CFI_AMD_PROGRAM		0xa0
 #define	CFI_AMD_UNLOCK		0xaa
+#define	CFI_AMD_UNLOCK_BYPASS	0x20
+#define	CFI_AMD_RESET		0xf0
 
-#define	AMD_ADDR_START		0xaaa
-#define	AMD_ADDR_ACK		0x555
+#define	AMD_ADDR_START		0x555
+#define	AMD_ADDR_ACK		0x2aa
 
 #endif /* _DEV_CFI_REG_H_ */
Index: sys/dev/cfi/cfi_bus_siba.c
===================================================================
--- sys/dev/cfi/cfi_bus_siba.c	(revision 0)
+++ sys/dev/cfi/cfi_bus_siba.c	(revision 0)
@@ -0,0 +1,72 @@
+/*-
+ * Copyright (c) 2007, Juniper Networks, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the author nor the names of any co-contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/conf.h>
+#include <sys/kernel.h>
+#include <sys/malloc.h>   
+#include <sys/module.h>
+#include <sys/rman.h>
+#include <sys/sysctl.h>
+
+#include <machine/bus.h>
+
+#include <dev/cfi/cfi_var.h>
+
+
+static int cfi_siba_probe(device_t);
+
+static device_method_t cfi_siba_methods[] = {
+	/* device interface */
+	DEVMETHOD(device_probe,		cfi_siba_probe),
+	DEVMETHOD(device_attach,	cfi_attach),
+	DEVMETHOD(device_detach,	cfi_detach),
+
+	{0, 0}
+};
+
+static driver_t cfi_siba_driver = {
+	cfi_driver_name,
+	cfi_siba_methods,
+	sizeof(struct cfi_softc),
+};
+
+DRIVER_MODULE (cfi, siba_cc, cfi_siba_driver, cfi_devclass, 0, 0);
+
+static int
+cfi_siba_probe(device_t dev)
+{
+
+	return (cfi_probe(dev));
+}

Property changes on: sys/dev/cfi/cfi_bus_siba.c
___________________________________________________________________
Added: svn:mime-type
   + text/plain
Added: svn:keywords
   + FreeBSD=%H
Added: svn:eol-style
   + native

Index: sys/dev/cfi/cfi_core.c
===================================================================
--- sys/dev/cfi/cfi_core.c	(revision 219085)
+++ sys/dev/cfi/cfi_core.c	(working copy)
@@ -47,28 +47,94 @@
 #include <dev/cfi/cfi_reg.h>
 #include <dev/cfi/cfi_var.h>
 
+SYSCTL_NODE(_hw, OID_AUTO, cfi, CTLFLAG_RD, 0, "CFI driver parameters");
+
+/* "hw.cfi.rdonly" if set any writes are ignored, default "1" */
+static unsigned int cfi_rdonly = 1;
+SYSCTL_INT(_hw_cfi, OID_AUTO, rdonly, CTLFLAG_RW, &cfi_rdonly, 0,
+    "CFI Not allowed to write");
+TUNABLE_INT("hw.cfi.rdonly", &cfi_rdonly);
+
+/*
+ * "hw.cfi.protect_loader" - set top bound of bootloader region, bottom
+ * always 0. Default 0x40000 (256k). To disable - just set to "0".
+ */
+static unsigned int cfi_protect_loader = 0x40000;
+SYSCTL_INT(_hw_cfi, OID_AUTO, protect_loader, CTLFLAG_RW, &cfi_protect_loader,
+    0x40000, "Protect data from 0 to this offset");
+TUNABLE_INT("hw.cfi.protect_loader", &cfi_protect_loader);
+
+#define CFI_DEBUG
+
+#ifdef CFI_DEBUG
+
+enum CFI_DEBUG_FLAGS {
+	CFI_DEBUG_NONE 		= 0,
+	CFI_DEBUG_BLOCK_ERASE 	= (1<<0),
+	CFI_DEBUG_BLOCK_WRITE 	= (1<<1),
+	CFI_DEBUG_WORD_WRITE 	= (1<<2),
+        CFI_DEBUG_CMD 		= (1<<3),
+	CFI_DEBUG_DISABLE_WRITE	= (1<<4),
+        CFI_DEBUG_WAIT 		= (1<<5),
+        CFI_DEBUG_TIMEOUT_WAIT 	= (1<<6),
+        CFI_DEBUG_DO_RETRY 	= (1<<7)
+};
+
+static unsigned int cfi_debug = 0;
+SYSCTL_INT(_hw_cfi, OID_AUTO, debug, CTLFLAG_RW, &cfi_debug, 0, 
+    "CFI debug level");
+TUNABLE_INT("hw.cfi.debug", &cfi_debug);
+#endif
+
 extern struct cdevsw cfi_cdevsw;
 
 char cfi_driver_name[] = "cfi";
 devclass_t cfi_devclass;
 devclass_t cfi_diskclass;
 
-uint32_t
-cfi_read(struct cfi_softc *sc, u_int ofs)
+/*
+ * cfi_read(struct cfi_softc *sc, u_int block_addr, u_int ofs)
+ * block_addr applied without shift
+ * ofs applied with shift detected by cfi_detect_first
+ */
+
+u_int
+cfi_read(struct cfi_softc *sc, u_int block_addr, u_int ofs)
 {
-	uint32_t val;
+	u_int val;
 
-	ofs &= ~(sc->sc_width - 1);
 	switch (sc->sc_width) {
 	case 1:
-		val = bus_space_read_1(sc->sc_tag, sc->sc_handle, ofs);
+		val = bus_space_read_1(sc->sc_tag, sc->sc_handle, 
+		    block_addr | (ofs * sc->sc_shift));
+#ifdef CFI_DEBUG
+		if (cfi_debug & CFI_DEBUG_WORD_WRITE)
+			printf("READ1(%08x) = %02x)\n",
+			    block_addr | (ofs * sc->sc_shift), val);
+#endif
 		break;
 	case 2:
-		val = bus_space_read_2(sc->sc_tag, sc->sc_handle, ofs);
+		val = bus_space_read_2(sc->sc_tag, sc->sc_handle, 
+		    (block_addr | (ofs * sc->sc_shift)) & ~(2-1));
+#ifdef CFI_DEBUG
+		if (cfi_debug & CFI_DEBUG_WORD_WRITE)
+			printf("READ2(%08x) = %04x)\n",
+			    (block_addr | (ofs * sc->sc_shift)) & ~(2-1), val);
+#endif
 		break;
 	case 4:
-		val = bus_space_read_4(sc->sc_tag, sc->sc_handle, ofs);
+		val = bus_space_read_4(sc->sc_tag, sc->sc_handle, 
+		    (block_addr | (ofs * sc->sc_shift)) & ~(4-1));
+#ifdef CFI_DEBUG
+		if (cfi_debug & CFI_DEBUG_WORD_WRITE)
+			printf("READ4(%08x) = %08x)\n",
+			    (block_addr | (ofs * sc->sc_shift)) & ~(4-1), val);
+#endif
 		break;
+	case 8:
+		val = bus_space_read_8(sc->sc_tag, sc->sc_handle, 
+		    (block_addr | (ofs * sc->sc_shift)) & ~(8-1));
+		break;
 	default:
 		val = ~0;
 		break;
@@ -76,42 +142,90 @@
 	return (val);
 }
 
+
 static void
-cfi_write(struct cfi_softc *sc, u_int ofs, u_int val)
+cfi_write(struct cfi_softc *sc, u_int block_addr, u_int ofs, u_int val)
 {
 
-	ofs &= ~(sc->sc_width - 1);
 	switch (sc->sc_width) {
 	case 1:
-		bus_space_write_1(sc->sc_tag, sc->sc_handle, ofs, val);
+#ifdef CFI_DEBUG
+		if (cfi_debug & CFI_DEBUG_WORD_WRITE)
+			printf("WRITE1(%08x, %02x)\n",
+			    block_addr | (ofs * sc->sc_shift), val);
+		if (!(cfi_debug & CFI_DEBUG_DISABLE_WRITE))
+#endif
+			bus_space_write_1(sc->sc_tag, sc->sc_handle, 
+			    block_addr | (ofs * sc->sc_shift), val);
 		break;
 	case 2:
-		bus_space_write_2(sc->sc_tag, sc->sc_handle, ofs, val);
+#ifdef CFI_DEBUG
+		if (cfi_debug & CFI_DEBUG_WORD_WRITE)
+			printf("WRITE2(%08x, %04x)\n",
+			    (block_addr | (ofs * sc->sc_shift)) & ~(2-1), val);
+		if (!(cfi_debug & CFI_DEBUG_DISABLE_WRITE))
+#endif
+			bus_space_write_2(sc->sc_tag, sc->sc_handle, 
+			    (block_addr | (ofs * sc->sc_shift)) & ~(2-1), val);
 		break;
 	case 4:
-		bus_space_write_4(sc->sc_tag, sc->sc_handle, ofs, val);
+#ifdef CFI_DEBUG
+		if (cfi_debug & CFI_DEBUG_WORD_WRITE)
+			printf("WRITE4(%08x, %04x)\n",
+			    (block_addr | (ofs * sc->sc_shift)) & ~(4-1), val);
+		if (!(cfi_debug & CFI_DEBUG_DISABLE_WRITE))
+#endif
+			bus_space_write_4(sc->sc_tag, sc->sc_handle, 
+			    (block_addr | (ofs * sc->sc_shift)) & ~(4-1), val);
 		break;
+	case 8:
+		bus_space_write_8(sc->sc_tag, sc->sc_handle, 
+		    (block_addr | (ofs * sc->sc_shift)) & ~(8-1), val);
+		break;
 	}
 }
 
-uint8_t
-cfi_read_qry(struct cfi_softc *sc, u_int ofs)
+static u_int
+cfi_make_cmd(uint8_t cmd, u_int mask)
 {
-	uint8_t val;
- 
-	cfi_write(sc, CFI_QRY_CMD_ADDR * sc->sc_width, CFI_QRY_CMD_DATA); 
-	val = cfi_read(sc, ofs * sc->sc_width);
-	cfi_write(sc, 0, CFI_BCS_READ_ARRAY);
-	return (val);
-} 
+	int i;
+	u_int data = 0;
 
+	for (i = 0; i < sizeof(int); i ++) {
+		if (mask & (0xff << (i*8)))
+			data |= cmd << (i*8);
+	}
+
+	return (data);
+}
+
+/*
+ * cfi_array_write
+ * fill "bus width" word with value of var data by array mask sc->sc_mask
+ */
 static void
+cfi_array_write(struct cfi_softc *sc, u_int ofs, u_int addr, u_int data)
+{
+	data &= 0xff;
+	cfi_write(sc, ofs, addr, cfi_make_cmd(data, sc->sc_mask));
+}
+
+/*
+ * cfi_amd_write
+ * AMD unlock sequence first
+ */
+static void
 cfi_amd_write(struct cfi_softc *sc, u_int ofs, u_int addr, u_int data)
 {
-
-	cfi_write(sc, ofs + AMD_ADDR_START, CFI_AMD_UNLOCK);
-	cfi_write(sc, ofs + AMD_ADDR_ACK, CFI_AMD_UNLOCK_ACK);
-	cfi_write(sc, ofs + addr, data);
+#ifdef CFI_DEBUG
+	if ( (cfi_debug & CFI_DEBUG_CMD) && (ofs < cfi_protect_loader)) {
+		printf("cfi_amd_write to offset(0x%08x) less than "
+		    "cfi_protect_loader(0x%08x)\n", ofs, cfi_protect_loader);
+	}
+#endif
+	cfi_array_write(sc, ofs, AMD_ADDR_START, CFI_AMD_UNLOCK);
+	cfi_array_write(sc, ofs, AMD_ADDR_ACK, CFI_AMD_UNLOCK_ACK);
+	cfi_array_write(sc, ofs, addr, data);
 }
 
 static char *
@@ -131,13 +245,161 @@
 	return (buf);
 }
 
+static int
+cfi_do_query(struct cfi_softc *sc, u_int ofs, u_int mask)
+{
+	u_int q_data = 0, r_data = 0, y_data = 0;
+
+#ifdef CFI_DEBUG
+	printf("%s: query at %p with mask %p (w=%d, s=%d)\n", __func__,
+	    (void *)ofs, (void *)mask, sc->sc_width, sc->sc_shift);
+#endif
+	q_data = cfi_make_cmd('Q', mask);
+	r_data = cfi_make_cmd('R', mask);
+	y_data = cfi_make_cmd('Y', mask);
+
+	/* Reset to Read Array mode */
+	/* We don't know array mask yet */
+	cfi_write(sc, ofs, 0, cfi_make_cmd(CFI_BCS_READ_ARRAY, mask));
+	cfi_write(sc, ofs, 0, cfi_make_cmd(CFI_AMD_RESET, mask));
+
+	/* Try to pat into CFI QRY mode */
+	cfi_write(sc, ofs, CFI_QRY_CMD_ADDR, 
+	    cfi_make_cmd(CFI_QRY_CMD_DATA, mask));
+
+#ifdef CFI_DEBUG
+	printf("%s: cfi_read(sc, ofs, CFI_QRY_IDENT) return %p\n", __func__,
+	    (void *)cfi_read(sc, ofs, CFI_QRY_IDENT));
+#endif
+	/* Check for 'Q', 'R', 'Y' chars */
+	if (
+	    (cfi_read(sc, ofs, CFI_QRY_IDENT    ) & mask) == q_data &&
+	    (cfi_read(sc, ofs, CFI_QRY_IDENT + 1) & mask) == r_data &&
+	    (cfi_read(sc, ofs, CFI_QRY_IDENT + 2) & mask) == y_data ) {
+		/* If we have answer on offset 0 too */
+		if ( ofs &&
+		    (cfi_read(sc, 0, CFI_QRY_IDENT    ) & mask) == q_data &&
+		    (cfi_read(sc, 0, CFI_QRY_IDENT + 1) & mask) == r_data &&
+		    (cfi_read(sc, 0, CFI_QRY_IDENT + 2) & mask) == y_data )
+			/* current chip only alias of first */
+			return (1);
+		/* Flash found */
+		return (0);
+	}
+
+	/* Return to Read Array mode */
+	cfi_write(sc, ofs, 0, cfi_make_cmd(CFI_BCS_READ_ARRAY, mask));
+	cfi_write(sc, ofs, 0, cfi_make_cmd(CFI_AMD_RESET, mask));
+
+	/* Flash not found */
+	return (1);
+}
+
+static int
+cfi_detect_first(struct cfi_softc *sc)
+{
+	u_int	width;
+	u_int	shift;
+	u_int	unit;
+
+	unit = device_get_unit(sc->sc_dev);
+	resource_int_value("cfi", unit, "width", &sc->sc_width);
+	resource_int_value("cfi", unit, "shift", &sc->sc_shift);
+
+	/*
+	 * XXX maybe detection algorithm must step w4:s4, w2:s2, w1:s1 first.
+	 * then w4:s2, w2:s4, etc.
+	 */
+
+	/* Start from longest supported width */
+	for (width = ((sc->sc_width == 0)?sizeof(int):sc->sc_width); width;
+		  width >>= 1) {
+
+		sc->sc_width = width;
+
+		for (shift = ((sc->sc_shift == 0)?sizeof(int):sc->sc_shift);
+			  shift; shift >>= 1) {
+
+			sc->sc_shift = shift;
+
+			if (cfi_do_query(sc, 0, 0xff) == 0)
+				return (0);
+		}
+	}
+
+	return (1);
+}
+
+static int
+cfi_detect_array(struct cfi_softc *sc)
+{
+	int i, c, first;
+	u_int reset = 0, value, mask = 0;
+
+	/* Reset all chips in row */
+	for (i = 0; i < sc->sc_width; i ++) {
+		reset |= CFI_BCS_READ_ARRAY << (i*8);
+	}
+
+	cfi_write(sc, 0, 0, reset);
+	for (i = 0; i < sc->sc_width; i ++) {
+		reset |= CFI_AMD_RESET << (i*8);
+	}
+
+	cfi_write(sc, 0, 0, reset);
+
+	for (i = 0; i < sc->sc_width; i ++) {
+		/* Try everyone separate */
+		cfi_write(sc, 0, CFI_QRY_CMD_ADDR, CFI_QRY_CMD_DATA << (i*8));
+		value = cfi_read(sc, 0, CFI_QRY_IDENT);
+
+		first = 1;
+		for (c = 0; c < sc->sc_width; c ++) {
+			if (((value >> (c*8)) & 0xff) == 'Q' && first) {
+				mask |= 0xff<<(c*8);
+				first = 0;
+			}
+		}
+		/* 
+		 * result must be:
+		 * 0x000000ff for 1x8bit, 1x16bit, 1x32bit
+		 * 0x0000ffff for 2x8bit
+		 * 0x00ff00ff for 2x16bit
+		 * 0xffffffff for 4x8bit
+		 * 64bit not shown
+		 */
+
+	}
+
+	cfi_write(sc, 0, 0, reset);
+
+	if (bootverbose)
+		device_printf(sc->sc_dev, "Array mask = %p\n", (void *)mask);
+
+	sc->sc_mask = mask;
+
+	/* Test next row */
+	for (sc->sc_rows = 1; (sc->sc_rows * sc->sc_size) < sc->sc_winsize;
+	    sc->sc_rows++) {
+		if (cfi_do_query(sc, sc->sc_rows * sc->sc_size, mask)) {
+			/*
+			 * If now QRY detected, then no more rows,
+			 * just return
+			 */
+			return (0);
+		}
+	}
+
+	return (0);
+}
+
 int
 cfi_probe(device_t dev)
 {
 	char desc[80];
 	struct cfi_softc *sc;
 	char *vend_str;
-	int error;
+	int error, i;
 	uint16_t iface, vend;
 
 	sc = device_get_softc(dev);
@@ -151,33 +413,24 @@
 
 	sc->sc_tag = rman_get_bustag(sc->sc_res);
 	sc->sc_handle = rman_get_bushandle(sc->sc_res);
+	sc->sc_winsize = rman_get_size(sc->sc_res);
 
-	if (sc->sc_width == 0) {
-		sc->sc_width = 1;
-		while (sc->sc_width <= 4) {
-			if (cfi_read_qry(sc, CFI_QRY_IDENT) == 'Q')
-				break;
-			sc->sc_width <<= 1;
-		}
-	} else if (cfi_read_qry(sc, CFI_QRY_IDENT) != 'Q') {
+	if (cfi_detect_first(sc)) {
 		error = ENXIO;
 		goto out;
 	}
-	if (sc->sc_width > 4) {
-		error = ENXIO;
-		goto out;
-	}
 
-	/* We got a Q. Check if we also have the R and the Y. */
-	if (cfi_read_qry(sc, CFI_QRY_IDENT + 1) != 'R' ||
-	    cfi_read_qry(sc, CFI_QRY_IDENT + 2) != 'Y') {
-		error = ENXIO;
-		goto out;
-	}
+	if (bootverbose)
+		device_printf(dev, "detected with bus width = %d, "
+		    "address shift = %d\n", sc->sc_width, sc->sc_shift);
 
+	/* Ask only first chip in first fow */
+	cfi_write(sc, 0, CFI_QRY_CMD_ADDR, CFI_QRY_CMD_DATA);
+
+
 	/* Get the vendor and command set. */
-	vend = cfi_read_qry(sc, CFI_QRY_VEND) |
-	    (cfi_read_qry(sc, CFI_QRY_VEND + 1) << 8);
+	vend = cfi_read(sc, 0, CFI_QRY_PRI_VEND_CMDSET) |
+	    (cfi_read(sc, 0, CFI_QRY_PRI_VEND_CMDSET + 1) << 8);
 
 	sc->sc_cmdset = vend;
 
@@ -185,45 +438,69 @@
 	case CFI_VEND_AMD_ECS:
 	case CFI_VEND_AMD_SCS:
 		vend_str = "AMD/Fujitsu";
+		sc->sc_rstcmd = CFI_AMD_RESET;
 		break;
 	case CFI_VEND_INTEL_ECS:
 		vend_str = "Intel/Sharp";
+		sc->sc_rstcmd = CFI_BCS_READ_ARRAY;
 		break;
 	case CFI_VEND_INTEL_SCS:
 		vend_str = "Intel";
+		sc->sc_rstcmd = CFI_BCS_READ_ARRAY;
 		break;
 	case CFI_VEND_MITSUBISHI_ECS:
 	case CFI_VEND_MITSUBISHI_SCS:
 		vend_str = "Mitsubishi";
+		sc->sc_rstcmd = CFI_BCS_READ_ARRAY;
 		break;
 	default:
 		vend_str = "Unknown vendor";
+		sc->sc_rstcmd = CFI_AMD_RESET;
 		break;
 	}
 
 	/* Get the device size. */
-	sc->sc_size = 1U << cfi_read_qry(sc, CFI_QRY_SIZE);
+	sc->sc_size = 1U << cfi_read(sc, 0, CFI_QRY_SIZE);
 
 	/* Sanity-check the I/F */
-	iface = cfi_read_qry(sc, CFI_QRY_IFACE) |
-	    (cfi_read_qry(sc, CFI_QRY_IFACE + 1) << 8);
+	iface = cfi_read(sc, 0, CFI_QRY_IFACE) |
+	    (cfi_read(sc, 0, CFI_QRY_IFACE + 1) << 8);
 
-	/*
-	 * Adding 1 to iface will give us a bit-wise "switch"
-	 * that allows us to test for the interface width by
-	 * testing a single bit.
-	 */
-	iface++;
+	cfi_write(sc, 0, 0, sc->sc_rstcmd);
+	switch (sc->sc_cmdset) {
+	case CFI_VEND_AMD_ECS:
+	case CFI_VEND_AMD_SCS:
+		cfi_amd_write(sc, 0, AMD_ADDR_START, CFI_AMD_GET_ID);
+		sc->sc_manid = cfi_read(sc, 0, CFI_AMD_MANID_ADDR) & 0xff;
+		sc->sc_devid = cfi_read(sc, 0, CFI_AMD_DEVID_ADDR) & 0xff;
+		cfi_write(sc, 0, 0, CFI_AMD_RESET);
+		if (bootverbose)
+			device_printf(dev, 
+			    "CFI Flash Manufacturer ID = 0x%02x, "
+			    "Device ID 0x%02x\n",
+			    sc->sc_manid, sc->sc_devid);
+		break;
+	}
 
-	error = (iface & sc->sc_width) ? 0 : EINVAL;
-	if (error)
-		goto out;
+	/* Check if we have interleaved or more than one in sequence */
+	cfi_detect_array(sc);
 
+	/* Now we know array geometry */
+	/* Get number of chips in a row */
+	for (i = 0; i < sc->sc_width; i++)
+		if (sc->sc_mask & (0xff << (i*8)))
+			sc->sc_array_width++;
+
+	if (sc->sc_rows > 1 || sc->sc_array_width > 1)
+		device_printf(dev, "Flash array [width=%d:rows=%d] detected\n",
+		    sc->sc_array_width, sc->sc_rows);
+
 	snprintf(desc, sizeof(desc), "%s - %s", vend_str,
-	    cfi_fmtsize(sc->sc_size));
+	    cfi_fmtsize(sc->sc_size * sc->sc_rows));
 	device_set_desc_copy(dev, desc);
+	error = 0;
 
- out:
+out:
 	bus_release_resource(dev, SYS_RES_MEMORY, sc->sc_rid, sc->sc_res);
 	return (error);
 }
@@ -233,7 +510,7 @@
 {
 	struct cfi_softc *sc;
 	u_int blksz, blocks;
-	u_int r, u;
+	u_int r, u, row;
 
 	sc = device_get_softc(dev);
 	sc->sc_dev = dev;
@@ -247,29 +524,51 @@
 	sc->sc_tag = rman_get_bustag(sc->sc_res);
 	sc->sc_handle = rman_get_bushandle(sc->sc_res);
 
+	cfi_array_write(sc, 0, 0, sc->sc_rstcmd);
+	cfi_array_write(sc, 0, CFI_QRY_CMD_ADDR, CFI_QRY_CMD_DATA);
+
 	/* Get time-out values for erase and write. */
-	sc->sc_write_timeout = 1 << cfi_read_qry(sc, CFI_QRY_TTO_WRITE);
-	sc->sc_erase_timeout = 1 << cfi_read_qry(sc, CFI_QRY_TTO_ERASE);
-	sc->sc_write_timeout *= 1 << cfi_read_qry(sc, CFI_QRY_MTO_WRITE);
-	sc->sc_erase_timeout *= 1 << cfi_read_qry(sc, CFI_QRY_MTO_ERASE);
+	sc->sc_write_timeout = 1 << cfi_read(sc, 0, CFI_QRY_TTO_WRITE);
+	sc->sc_erase_timeout = 1 << cfi_read(sc, 0, CFI_QRY_TTO_ERASE);
+	sc->sc_write_max_timeout = 1 << cfi_read(sc, 0, CFI_QRY_MTO_WRITE);
+	sc->sc_erase_max_timeout = 1 << cfi_read(sc, 0, CFI_QRY_MTO_ERASE);
+	if (bootverbose) {
+		device_printf(dev, "\n");
+	}
+	sc->sc_erase_timeout *= 1000; /* ms */
 
+
 	/* Get erase regions. */
-	sc->sc_regions = cfi_read_qry(sc, CFI_QRY_NREGIONS);
-	sc->sc_region = malloc(sc->sc_regions * sizeof(struct cfi_region),
-	    M_TEMP, M_WAITOK | M_ZERO);
+	sc->sc_regions = cfi_read(sc, 0, CFI_QRY_NREGIONS);
+	sc->sc_region = malloc(sc->sc_regions * sc->sc_rows *
+	    sizeof(struct cfi_region), M_TEMP, M_WAITOK | M_ZERO);
+
 	for (r = 0; r < sc->sc_regions; r++) {
-		blocks = cfi_read_qry(sc, CFI_QRY_REGION(r)) |
-		    (cfi_read_qry(sc, CFI_QRY_REGION(r) + 1) << 8);
+		blocks = cfi_read(sc, 0, CFI_QRY_REGION(r)) |
+		    (cfi_read(sc, 0, CFI_QRY_REGION(r) + 1) << 8);
 		sc->sc_region[r].r_blocks = blocks + 1;
 
-		blksz = cfi_read_qry(sc, CFI_QRY_REGION(r) + 2) |
-		    (cfi_read_qry(sc, CFI_QRY_REGION(r) + 3) << 8);
+		blksz = cfi_read(sc, 0, CFI_QRY_REGION(r) + 2) |
+		    (cfi_read(sc, 0, CFI_QRY_REGION(r) + 3) << 8);
 		sc->sc_region[r].r_blksz = (blksz == 0) ? 128 :
 		    blksz * 256;
 	}
 
+	/* Second chip not always says your info, just copy from first */
+	for (row = 0; row < sc->sc_rows; row++) {
+		for (r = 0; r < sc->sc_regions; r++) {
+			sc->sc_region[r + row * sc->sc_regions].r_blocks =
+			    sc->sc_region[r].r_blocks;
+			sc->sc_region[r + row * sc->sc_regions].r_blksz  =
+			    sc->sc_region[r].r_blksz * sc->sc_array_width;
+		}
+	}
+
+	sc->sc_regions *= sc->sc_rows;
+	sc->sc_size *= (sc->sc_rows * sc->sc_array_width);
+
 	/* Reset the device to a default state. */
-	cfi_write(sc, 0, CFI_BCS_CLEAR_STATUS);
+	cfi_array_write(sc, 0, 0, sc->sc_rstcmd);
 
 	if (bootverbose) {
 		device_printf(dev, "[");
@@ -305,33 +604,55 @@
 }
 
 static int
-cfi_wait_ready(struct cfi_softc *sc, u_int ofs, u_int timeout)
+cfi_wait_ready(struct cfi_softc *sc, u_int ofs, u_int timeout, u_int count)
 {
 	int done, error;
-	uint32_t st0 = 0, st = 0;
+	u_int st0 = 0, st = 0;
 
 	done = 0;
 	error = 0;
-	timeout *= 10;
-	while (!done && !error && timeout) {
-		DELAY(100);
-		timeout--;
 
+#ifdef CFI_DEBUG
+	if (cfi_debug & 0xf0000000)
+		timeout *= ((cfi_debug & 0xf0000000) >> 28);
+#endif
+#ifdef CFI_DEBUG
+	if (cfi_debug & CFI_DEBUG_WAIT)
+		printf("%s: ofs = %p, timeout = %d, count = %d\n", __func__,
+		    (void *)ofs, timeout, count);
+#endif
+
+	if (!timeout)	timeout = 100;	/* Default to 100 uS */
+	if (!count)	count = 100;	/* Max timeout is 10 mS */
+
+	while (!done && !error && count) {
+		DELAY(timeout);
+
+#ifdef CFI_DEBUG
+		if (cfi_debug & CFI_DEBUG_TIMEOUT_WAIT)
+			printf("%s: ofs = %p, count = %d\n", __func__,
+			    (void *)ofs, count);
+#endif
+		count--;
+
 		switch (sc->sc_cmdset) {
 		case CFI_VEND_INTEL_ECS:
 		case CFI_VEND_INTEL_SCS:
-			st = cfi_read(sc, ofs);
+			st = cfi_read(sc, ofs, 0);
 			done = (st & CFI_INTEL_STATUS_WSMS);
 			if (done) {
 				/* NB: bit 0 is reserved */
 				st &= ~(CFI_INTEL_XSTATUS_RSVD |
 					CFI_INTEL_STATUS_WSMS |
 					CFI_INTEL_STATUS_RSVD);
-				if (st & CFI_INTEL_STATUS_DPS)
+				if (st & cfi_make_cmd(
+				    CFI_INTEL_STATUS_DPS, sc->sc_mask))
 					error = EPERM;
-				else if (st & CFI_INTEL_STATUS_PSLBS)
+				else if (st & cfi_make_cmd(
+				    CFI_INTEL_STATUS_PSLBS, sc->sc_mask))
 					error = EIO;
-				else if (st & CFI_INTEL_STATUS_ECLBS)
+				else if (st & cfi_make_cmd(
+				    CFI_INTEL_STATUS_ECLBS, sc->sc_mask))
 					error = ENXIO;
 				else if (st)
 					error = EACCES;
@@ -339,53 +660,137 @@
 			break;
 		case CFI_VEND_AMD_SCS:
 		case CFI_VEND_AMD_ECS:
-			st0 = cfi_read(sc, ofs);
-			st = cfi_read(sc, ofs);
-			done = ((st & 0x40) == (st0 & 0x40)) ? 1 : 0;
+			/*
+			 * read sc->sc_width bytes, and check for toggle bit.
+			 */
+			st0 = cfi_read(sc, ofs, 0);
+			st = cfi_read(sc, ofs, 0);
+			done = ((st & cfi_make_cmd(0x40, sc->sc_mask)) == 
+			    (st0 & cfi_make_cmd(0x40, sc->sc_mask))) ? 1 : 0;
+
 			break;
 		}
 	}
 	if (!done && !error)
 		error = ETIMEDOUT;
 	if (error)
-		printf("\nerror=%d (st 0x%x st0 0x%x)\n", error, st, st0);
+		printf("\nerror=%d (st 0x%x st0 0x%x) at offset=%p\n", 
+		    error, st, st0, (void *)ofs);
 	return (error);
 }
 
 int
+cfi_erase_block(struct cfi_softc *sc, u_int offset)
+{
+	int error = 0;
+
+#ifdef CFI_DEBUG
+	if (cfi_debug & CFI_DEBUG_BLOCK_ERASE)
+		printf("%s: offset = %p\n", __func__, (void *)offset);
+#endif
+
+	if (offset > sc->sc_size)
+		panic("CFI: erase offset (%p) bigger "
+		    "than cfi array size (%p)\n", 
+		    (void *)sc->sc_wrofs, (void *)sc->sc_size);
+
+	/* Erase the block. */
+	switch (sc->sc_cmdset) {
+	case CFI_VEND_INTEL_ECS:
+	case CFI_VEND_INTEL_SCS:
+		cfi_array_write(sc, offset, 0, CFI_BCS_BLOCK_ERASE);
+		cfi_array_write(sc, offset, 0, CFI_BCS_CONFIRM);
+		break;
+	case CFI_VEND_AMD_SCS:
+	case CFI_VEND_AMD_ECS:
+		cfi_amd_write(sc, offset, AMD_ADDR_START, CFI_AMD_ERASE_SECTOR);
+		cfi_amd_write(sc, offset, 0, CFI_AMD_BLOCK_ERASE);
+		break;
+	default:
+		/* Better safe than sorry... */
+		return (ENODEV);
+	}
+#ifdef CFI_DEBUG
+	if (cfi_debug & CFI_DEBUG_BLOCK_ERASE)
+		printf("%s: cfi_wait_ready(sc, offset = %p, timout = %d uS, "
+		    "count = %d   %lld)\n", __func__, (void *)offset, 
+		    sc->sc_erase_timeout, sc->sc_erase_max_timeout, cpu_ticks());
+#endif
+	error = cfi_wait_ready(sc, offset, sc->sc_erase_timeout,
+	    sc->sc_erase_max_timeout);
+#ifdef CFI_DEBUG
+	if (cfi_debug & CFI_DEBUG_BLOCK_ERASE)
+		printf("%s: cfi_wait_ready(sc, offset = %p, timout = %d uS, "
+		    "count = %d   %lld) done\n", __func__, (void *)offset, 
+		    sc->sc_erase_timeout, sc->sc_erase_max_timeout, cpu_ticks());
+#endif
+
+	return (error);
+}
+
+#define CFI_AMD_BYPASS 0
+
+int
 cfi_write_block(struct cfi_softc *sc)
 {
 	union {
 		uint8_t		*x8;
 		uint16_t	*x16;
 		uint32_t	*x32;
+		uint64_t	*x64;
 	} ptr;
 	register_t intr;
 	int error, i;
 
-	/* Erase the block. */
+	if (sc->sc_wrofs > sc->sc_size)
+		panic("CFI: write offset (%p) bigger "
+		    "than cfi array size (%p)\n", 
+		    (void *)sc->sc_wrofs, (void *)sc->sc_size);
+
+#ifdef CFI_DEBUG
+	if (cfi_debug & CFI_DEBUG_BLOCK_WRITE)
+		device_printf(sc->sc_dev,"CFI write offset=0x%08x\n", 
+		    sc->sc_wrofs);
+#endif
+
+	if (cfi_rdonly)
+		return (EOPNOTSUPP);
+
+	if (( sc->sc_wrofs < cfi_protect_loader) ||
+	    ((sc->sc_wrofs + sc->sc_wrbufsz) < cfi_protect_loader)) {
+		device_printf(sc->sc_dev,"Attempt to write bootloader area "
+		    "at 0x%08x size 0x%08x\n", sc->sc_wrofs, sc->sc_wrbufsz);
+		return (EOPNOTSUPP);
+	}
+
+	error = cfi_erase_block(sc, sc->sc_wrofs);
+	if (error)
+		goto out;
+
+#ifdef CFI_DEBUG
+	if (cfi_debug & CFI_DEBUG_BLOCK_WRITE)
+		device_printf(sc->sc_dev,"CFI write start, offset=0x%08x\n",
+		    sc->sc_wrofs);
+#endif
+	/* Write the block. */
+	ptr.x8 = sc->sc_wrbuf;
+
+	/* Unlock Bypass. */
 	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:
+#if CFI_AMD_BYPASS
+		/* Unlock Bypass */
 		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_UNLOCK_BYPASS);
+#endif
 		break;
 	default:
 		/* Better safe than sorry... */
 		return (ENODEV);
 	}
-	error = cfi_wait_ready(sc, sc->sc_wrofs, sc->sc_erase_timeout);
-	if (error)
-		goto out;
 
-	/* Write the block. */
-	ptr.x8 = sc->sc_wrbuf;
+
 	for (i = 0; i < sc->sc_wrbufsz; i += sc->sc_width) {
 
 		/*
@@ -398,39 +803,52 @@
 		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_array_write(sc, sc->sc_wrofs + i, 0,
+			    CFI_BCS_PROGRAM);
 			break;
 		case CFI_VEND_AMD_SCS:
 		case CFI_VEND_AMD_ECS:
-			cfi_amd_write(sc, 0, AMD_ADDR_START, CFI_AMD_PROGRAM);
+#if CFI_AMD_BYPASS
+			/* Unlock Bypass Program */
+			cfi_write(sc, sc->sc_wrofs, 0, CFI_AMD_PROGRAM);
+#else
+			cfi_amd_write(sc, sc->sc_wrofs, AMD_ADDR_START,
+			    CFI_AMD_PROGRAM);
+#endif
 			break;
 		}
+		/* Raw data do not use cfi_array_write */
 		switch (sc->sc_width) {
 		case 1:
-			bus_space_write_1(sc->sc_tag, sc->sc_handle,
-			    sc->sc_wrofs + i, *(ptr.x8)++);
+			cfi_write(sc, sc->sc_wrofs + i, 0, *(ptr.x8)++);
 			break;
 		case 2:
-			bus_space_write_2(sc->sc_tag, sc->sc_handle,
-			    sc->sc_wrofs + i, *(ptr.x16)++);
+			cfi_write(sc, sc->sc_wrofs + i, 0, *(ptr.x16)++);
 			break;
 		case 4:
-			bus_space_write_4(sc->sc_tag, sc->sc_handle,
-			    sc->sc_wrofs + i, *(ptr.x32)++);
+			cfi_write(sc, sc->sc_wrofs + i, 0, *(ptr.x32)++);
 			break;
+		case 8:
+			cfi_write(sc, sc->sc_wrofs + i, 0, *(ptr.x64)++);
+			break;
 		}
 
 		intr_restore(intr);
 
-		error = cfi_wait_ready(sc, sc->sc_wrofs, sc->sc_write_timeout);
+		error = cfi_wait_ready(sc, sc->sc_wrofs + i, 
+		    sc->sc_write_timeout, sc->sc_write_max_timeout);
 		if (error)
 			goto out;
 	}
-
 	/* error is 0. */
 
  out:
-	cfi_write(sc, 0, CFI_BCS_READ_ARRAY);
+#if CFI_AMD_BYPASS
+	/* Unlock Bypass Reset */
+	cfi_array_write(sc, sc->sc_wrofs, 0, 0x90);
+	cfi_array_write(sc, sc->sc_wrofs, 0, 0x00);
+#endif
+	cfi_array_write(sc, sc->sc_wrofs, 0, sc->sc_rstcmd);
 	return (error);
 }
 
@@ -472,12 +890,12 @@
 		return EOPNOTSUPP;
 	KASSERT(sc->sc_width == 2, ("sc_width %d", sc->sc_width));
 
-	cfi_write(sc, 0, CFI_INTEL_READ_ID);
+	cfi_array_write(sc, 0, 0, CFI_INTEL_READ_ID);
 	*id = ((uint64_t)cfi_get16(sc, CFI_INTEL_PR(0)))<<48 |
 	      ((uint64_t)cfi_get16(sc, CFI_INTEL_PR(1)))<<32 |
 	      ((uint64_t)cfi_get16(sc, CFI_INTEL_PR(2)))<<16 |
 	      ((uint64_t)cfi_get16(sc, CFI_INTEL_PR(3)));
-	cfi_write(sc, 0, CFI_BCS_READ_ARRAY);
+	cfi_array_write(sc, 0, 0, CFI_BCS_READ_ARRAY);
 	return 0;
 }
 
@@ -491,12 +909,12 @@
 		return EOPNOTSUPP;
 	KASSERT(sc->sc_width == 2, ("sc_width %d", sc->sc_width));
 
-	cfi_write(sc, 0, CFI_INTEL_READ_ID);
+	cfi_array_write(sc, 0, 0, CFI_INTEL_READ_ID);
 	*id = ((uint64_t)cfi_get16(sc, CFI_INTEL_PR(4)))<<48 |
 	      ((uint64_t)cfi_get16(sc, CFI_INTEL_PR(5)))<<32 |
 	      ((uint64_t)cfi_get16(sc, CFI_INTEL_PR(6)))<<16 |
 	      ((uint64_t)cfi_get16(sc, CFI_INTEL_PR(7)));
-	cfi_write(sc, 0, CFI_BCS_READ_ARRAY);
+	cfi_array_write(sc, 0, 0, CFI_BCS_READ_ARRAY);
 	return 0;
 }
 
@@ -519,15 +937,15 @@
 #ifdef CFI_ARMEDANDDANGEROUS
 	for (i = 7; i >= 4; i--, id >>= 16) {
 		intr = intr_disable();
-		cfi_write(sc, 0, CFI_INTEL_PP_SETUP);
+		cfi_array_write(sc, 0, 0, CFI_INTEL_PP_SETUP);
 		cfi_put16(sc, CFI_INTEL_PR(i), id&0xffff);
 		intr_restore(intr);
 		error = cfi_wait_ready(sc, CFI_BCS_READ_STATUS,
-		    sc->sc_write_timeout);
+		    sc->sc_write_timeout, sc->sc_write_max_timeout);
 		if (error)
 			break;
 	}
-	cfi_write(sc, 0, CFI_BCS_READ_ARRAY);
+	cfi_array_write(sc, 0, 0, CFI_BCS_READ_ARRAY);
 	return error;
 #else
 	device_printf(sc->sc_dev, "%s: OEM PR not set, "
@@ -546,9 +964,9 @@
 		return EOPNOTSUPP;
 	KASSERT(sc->sc_width == 2, ("sc_width %d", sc->sc_width));
 
-	cfi_write(sc, 0, CFI_INTEL_READ_ID);
+	cfi_array_write(sc, 0, 0, CFI_INTEL_READ_ID);
 	*plr = cfi_get16(sc, CFI_INTEL_PLR);
-	cfi_write(sc, 0, CFI_BCS_READ_ARRAY);
+	cfi_array_write(sc, 0, 0, CFI_BCS_READ_ARRAY);
 	return 0;
 }
 
@@ -572,11 +990,11 @@
 	/* worthy of console msg */
 	device_printf(sc->sc_dev, "set PLR\n");
 	intr = intr_disable();
-	cfi_write(sc, 0, CFI_INTEL_PP_SETUP);
+	cfi_array_write(sc, 0, 0, CFI_INTEL_PP_SETUP);
 	cfi_put16(sc, CFI_INTEL_PLR, 0xFFFD);
 	intr_restore(intr);
-	error = cfi_wait_ready(sc, CFI_BCS_READ_STATUS, sc->sc_write_timeout);
-	cfi_write(sc, 0, CFI_BCS_READ_ARRAY);
+	error = cfi_wait_ready(sc, CFI_BCS_READ_STATUS, sc->sc_write_timeout, sc->sc_write_max_timeout);
+	cfi_array_write(sc, 0, 0, CFI_BCS_READ_ARRAY);
 	return error;
 #else
 	device_printf(sc->sc_dev, "%s: PLR not set, "
Index: sys/dev/cfi/cfi_disk.c
===================================================================
--- sys/dev/cfi/cfi_disk.c	(revision 219085)
+++ sys/dev/cfi/cfi_disk.c	(working copy)
@@ -57,7 +57,21 @@
 	struct task	iotask;		/* i/o processing */
 };
 
-#define	CFI_DISK_SECSIZE	512
+
+/*
+ * Max chip bus width is 4. 
+ * Data in flash maybe aligned on any block size,
+ * so we need less as posible sector size.
+ */
+
+/*
+ * XXX Require to rewrite some GEOM modules,
+ * they are read sectorsize for read signature,
+ * but need read rquired size aligned to sectorsize.
+ * Example: geom_label
+ */
+
+#define	CFI_DISK_SECSIZE	4
 #define	CFI_DISK_MAXIOSIZE	65536
 
 static int cfi_disk_detach(device_t);
@@ -182,19 +196,19 @@
 	if (sc->sc_width == 1) {
 		uint8_t *dp = (uint8_t *)bp->bio_data;
 		while (resid > 0 && bp->bio_offset < sc->sc_size) {
-			*dp++ = cfi_read(sc, bp->bio_offset);
+			*dp++ = cfi_read(sc, bp->bio_offset, 0);
 			bp->bio_offset += 1, resid -= 1;
 		}
 	} else if (sc->sc_width == 2) {
 		uint16_t *dp = (uint16_t *)bp->bio_data;
 		while (resid > 0 && bp->bio_offset < sc->sc_size) {
-			*dp++ = cfi_read(sc, bp->bio_offset);
+			*dp++ = cfi_read(sc, bp->bio_offset, 0);
 			bp->bio_offset += 2, resid -= 2;
 		}
 	} else {
 		uint32_t *dp = (uint32_t *)bp->bio_data;
 		while (resid > 0 && bp->bio_offset < sc->sc_size) {
-			*dp++ = cfi_read(sc, bp->bio_offset);
+			*dp++ = cfi_read(sc, bp->bio_offset, 0);
 			bp->bio_offset += 4, resid -= 4;
 		}
 	}

--Multipart=_Mon__14_Mar_2011_17_09_42_+0200_kl6/fo6bUqwAX_PL--



Want to link to this message? Use this URL: <https://mail-archive.FreeBSD.org/cgi/mid.cgi?20110314170942.90bfb5a8.ray>