[brcm-2.4] fix serial flash support (#6442)

git-svn-id: svn://svn.openwrt.org/openwrt/trunk@19171 3c298f89-4303-0410-b956-a3cf2f4a3e73

1 files changed, 254 insertions, 481 deletions

diff --git a/target/linux/brcm-2.4/files/drivers/mtd/devices/sflash.c b/target/linux/brcm-2.4/files/drivers/mtd/devices/sflash.c
index 7ffb004785..a8aab7bd97 100644
--- a/target/linux/brcm-2.4/files/drivers/mtd/devices/sflash.c
+++ b/target/linux/brcm-2.4/files/drivers/mtd/devices/sflash.c
@@ -1,530 +1,303 @@
 /*
  * Broadcom SiliconBackplane chipcommon serial flash interface
  *
- * Copyright 2007, Broadcom Corporation
- * All Rights Reserved.
- * 
- * THIS SOFTWARE IS OFFERED "AS IS", AND BROADCOM GRANTS NO WARRANTIES OF ANY
- * KIND, EXPRESS OR IMPLIED, BY STATUTE, COMMUNICATION OR OTHERWISE. BROADCOM
- * SPECIFICALLY DISCLAIMS ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS
- * FOR A SPECIFIC PURPOSE OR NONINFRINGEMENT CONCERNING THIS SOFTWARE.
+ * Copyright 2006, Broadcom Corporation      
+ * All Rights Reserved.      
+ *       
+ * THIS SOFTWARE IS OFFERED "AS IS", AND BROADCOM GRANTS NO WARRANTIES OF ANY      
+ * KIND, EXPRESS OR IMPLIED, BY STATUTE, COMMUNICATION OR OTHERWISE. BROADCOM      
+ * SPECIFICALLY DISCLAIMS ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS      
+ * FOR A SPECIFIC PURPOSE OR NONINFRINGEMENT CONCERNING THIS SOFTWARE.      
  *
+ * $Id$
  */
 
+#include <linux/config.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/ioport.h>
+#include <linux/mtd/compatmac.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/partitions.h>
+#include <linux/errno.h>
+#include <linux/pci.h>
+#include <linux/delay.h>
+#include <asm/io.h>
+
 #include <typedefs.h>
 #include <osl.h>
+// #include <bcmutils.h>
+#include <bcmdevs.h>
+#include <bcmnvram.h>
 #include <sbutils.h>
 #include <sbconfig.h>
 #include <sbchipc.h>
-#include <bcmdevs.h>
 #include <sflash.h>
 
+#ifdef CONFIG_MTD_PARTITIONS
+extern struct mtd_partition * init_mtd_partitions(struct mtd_info *mtd, size_t size);
+#endif
+
+struct sflash_mtd {
+	sb_t *sbh;
+	chipcregs_t *cc;
+	struct semaphore lock;
+	struct mtd_info mtd;
+	struct mtd_erase_region_info region;
+};
+
 /* Private global state */
-static struct sflash sflash;
+static struct sflash_mtd sflash;
 
-/* Issue a serial flash command */
-static INLINE void
-sflash_cmd (osl_t * osh, chipcregs_t * cc, uint opcode)
+static int
+sflash_mtd_poll(struct sflash_mtd *sflash, unsigned int offset, int timeout)
 {
-  W_REG (osh, &cc->flashcontrol, SFLASH_START | opcode);
-  while (R_REG (osh, &cc->flashcontrol) & SFLASH_BUSY);
-}
+	int now = jiffies;
+	int ret = 0;
 
-/* Initialize serial flash access */
-struct sflash *
-sflash_init (sb_t * sbh, chipcregs_t * cc)
-{
-  uint32 id, id2;
-  osl_t *osh;
-
-  ASSERT (sbh);
-
-  osh = sb_osh (sbh);
-
-  bzero (&sflash, sizeof (sflash));
-
-  sflash.type = sbh->cccaps & CC_CAP_FLASH_MASK;
-
-  switch (sflash.type)
-    {
-    case SFLASH_ST:
-      /* Probe for ST chips */
-      sflash_cmd (osh, cc, SFLASH_ST_DP);
-      sflash_cmd (osh, cc, SFLASH_ST_RES);
-      id = R_REG (osh, &cc->flashdata);
-      switch (id)
-	{
-	case 0x11:
-	  /* ST M25P20 2 Mbit Serial Flash */
-	  sflash.blocksize = 64 * 1024;
-	  sflash.numblocks = 4;
-	  break;
-	case 0x12:
-	  /* ST M25P40 4 Mbit Serial Flash */
-	  sflash.blocksize = 64 * 1024;
-	  sflash.numblocks = 8;
-	  break;
-	case 0x13:
-	  /* ST M25P80 8 Mbit Serial Flash */
-	  sflash.blocksize = 64 * 1024;
-	  sflash.numblocks = 16;
-	  break;
-	case 0x14:
-	  /* ST M25P16 16 Mbit Serial Flash */
-	  sflash.blocksize = 64 * 1024;
-	  sflash.numblocks = 32;
-	  break;
-	case 0x15:
-	  /* ST M25P32 32 Mbit Serial Flash */
-	  sflash.blocksize = 64 * 1024;
-	  sflash.numblocks = 64;
-	  break;
-	case 0x16:
-	  /* ST M25P64 64 Mbit Serial Flash */
-	  sflash.blocksize = 64 * 1024;
-	  sflash.numblocks = 128;
-	  break;
-	case 0xbf:
-	  W_REG (osh, &cc->flashaddress, 1);
-	  sflash_cmd (osh, cc, SFLASH_ST_RES);
-	  id2 = R_REG (osh, &cc->flashdata);
-	  if (id2 == 0x44)
-	    {
-	      /* SST M25VF80 4 Mbit Serial Flash */
-	      sflash.blocksize = 64 * 1024;
-	      sflash.numblocks = 8;
-	    }
-	  break;
-	}
-      break;
-
-    case SFLASH_AT:
-      /* Probe for Atmel chips */
-      sflash_cmd (osh, cc, SFLASH_AT_STATUS);
-      id = R_REG (osh, &cc->flashdata) & 0x3c;
-      switch (id)
-	{
-	case 0xc:
-	  /* Atmel AT45DB011 1Mbit Serial Flash */
-	  sflash.blocksize = 256;
-	  sflash.numblocks = 512;
-	  break;
-	case 0x14:
-	  /* Atmel AT45DB021 2Mbit Serial Flash */
-	  sflash.blocksize = 256;
-	  sflash.numblocks = 1024;
-	  break;
-	case 0x1c:
-	  /* Atmel AT45DB041 4Mbit Serial Flash */
-	  sflash.blocksize = 256;
-	  sflash.numblocks = 2048;
-	  break;
-	case 0x24:
-	  /* Atmel AT45DB081 8Mbit Serial Flash */
-	  sflash.blocksize = 256;
-	  sflash.numblocks = 4096;
-	  break;
-	case 0x2c:
-	  /* Atmel AT45DB161 16Mbit Serial Flash */
-	  sflash.blocksize = 512;
-	  sflash.numblocks = 4096;
-	  break;
-	case 0x34:
-	  /* Atmel AT45DB321 32Mbit Serial Flash */
-	  sflash.blocksize = 512;
-	  sflash.numblocks = 8192;
-	  break;
-	case 0x3c:
-	  /* Atmel AT45DB642 64Mbit Serial Flash */
-	  sflash.blocksize = 1024;
-	  sflash.numblocks = 8192;
-	  break;
+	for (;;) {
+		if (!sflash_poll(sflash->sbh, sflash->cc, offset)) {
+			ret = 0;
+			break;
+		}
+		if (time_after(jiffies, now + timeout)) {
+			printk(KERN_ERR "sflash: timeout\n");
+			ret = -ETIMEDOUT;
+			break;
+		}
+		if (current->need_resched) {
+			set_current_state(TASK_UNINTERRUPTIBLE);
+			schedule_timeout(timeout / 10);
+		} else
+			udelay(1);
 	}
-      break;
-    }
 
-  sflash.size = sflash.blocksize * sflash.numblocks;
-  return sflash.size ? &sflash : NULL;
+	return ret;
 }
 
-/* Read len bytes starting at offset into buf. Returns number of bytes read. */
-int
-sflash_read (sb_t * sbh, chipcregs_t * cc, uint offset, uint len, uchar * buf)
+static int
+sflash_mtd_read(struct mtd_info *mtd, loff_t from, size_t len, size_t *retlen, u_char *buf)
 {
-  uint8 *from, *to;
-  int cnt, i;
-  osl_t *osh;
-
-  ASSERT (sbh);
-
-  if (!len)
-    return 0;
+	struct sflash_mtd *sflash = (struct sflash_mtd *) mtd->priv;
+	int bytes, ret = 0;
+
+	/* Check address range */
+	if (len == 0){
+	 *retlen = 0;
+		return 0;
+ }
+	if (!len)
+		return 0;
+	if ((from + len) > mtd->size)
+		return -EINVAL;
+	
+	down(&sflash->lock);
+
+	*retlen = 0;
+	while (len) {
+		if ((bytes = sflash_read(sflash->sbh, sflash->cc, (uint) from, len, buf)) < 0) {
+			ret = bytes;
+			break;
+		}
+		from += (loff_t) bytes;
+		len -= bytes;
+		buf += bytes;
+		*retlen += bytes;
+	}
 
-  if ((offset + len) > sflash.size)
-    return -22;
+	up(&sflash->lock);
 
-  if ((len >= 4) && (offset & 3))
-    cnt = 4 - (offset & 3);
-  else if ((len >= 4) && ((uintptr) buf & 3))
-    cnt = 4 - ((uintptr) buf & 3);
-  else
-    cnt = len;
+	return ret;
+}
 
-  osh = sb_osh (sbh);
+static int
+sflash_mtd_write(struct mtd_info *mtd, loff_t to, size_t len, size_t *retlen, const u_char *buf)
+{
+	struct sflash_mtd *sflash = (struct sflash_mtd *) mtd->priv;
+	int bytes, ret = 0;
+
+	/* Check address range */
+	if (len == 0){
+	 *retlen = 0;
+		return 0;
+ }
+	if (!len)
+		return 0;
+	if ((to + len) > mtd->size)
+		return -EINVAL;
+
+	down(&sflash->lock);
+
+	*retlen = 0;
+	while (len) {
+		if ((bytes = sflash_write(sflash->sbh, sflash->cc, (uint)to, (uint)len, buf)) < 0) {
+			ret = bytes;
+			break;
+		}
+		if ((ret = sflash_mtd_poll(sflash, (unsigned int) to, HZ / 10)))
+			break;
+		to += (loff_t) bytes;
+		len -= bytes;
+		buf += bytes;
+		*retlen += bytes;
+	}
 
-  from = (uint8 *) (uintptr) OSL_UNCACHED (SB_FLASH2 + offset);
-  to = (uint8 *) buf;
+	up(&sflash->lock);
 
-  if (cnt < 4)
-    {
-      for (i = 0; i < cnt; i++)
-	{
-	  *to = R_REG (osh, from);
-	  from++;
-	  to++;
-	}
-      return cnt;
-    }
-
-  while (cnt >= 4)
-    {
-      *(uint32 *) to = R_REG (osh, (uint32 *) from);
-      from += 4;
-      to += 4;
-      cnt -= 4;
-    }
-
-  return (len - cnt);
+	return ret;
 }
 
-/* Poll for command completion. Returns zero when complete. */
-int
-sflash_poll (sb_t * sbh, chipcregs_t * cc, uint offset)
+static int
+sflash_mtd_erase(struct mtd_info *mtd, struct erase_info *erase)
 {
-  osl_t *osh;
-
-  ASSERT (sbh);
+	struct sflash_mtd *sflash = (struct sflash_mtd *) mtd->priv;
+	int i, j, ret = 0;
+	unsigned int addr, len;
+
+	/* Check address range */
+	if (!erase->len)
+		return 0;
+	if ((erase->addr + erase->len) > mtd->size)
+		return -EINVAL;
+
+	addr = erase->addr;
+	len = erase->len;
+
+	down(&sflash->lock);
+
+	/* Ensure that requested region is aligned */
+	for (i = 0; i < mtd->numeraseregions; i++) {
+		for (j = 0; j < mtd->eraseregions[i].numblocks; j++) {
+			if (addr == mtd->eraseregions[i].offset + mtd->eraseregions[i].erasesize * j &&
+			    len >= mtd->eraseregions[i].erasesize) {
+				if ((ret = sflash_erase(sflash->sbh, sflash->cc, addr)) < 0)
+					break;
+				if ((ret = sflash_mtd_poll(sflash, addr, 10 * HZ)))
+					break;
+				addr += mtd->eraseregions[i].erasesize;
+				len -= mtd->eraseregions[i].erasesize;
+			}
+		}
+		if (ret)
+			break;
+	}
 
-  osh = sb_osh (sbh);
+	up(&sflash->lock);
 
-  if (offset >= sflash.size)
-    return -22;
+	/* Set erase status */
+	if (ret)
+		erase->state = MTD_ERASE_FAILED;
+	else 
+		erase->state = MTD_ERASE_DONE;
 
-  switch (sflash.type)
-    {
-    case SFLASH_ST:
-      /* Check for ST Write In Progress bit */
-      sflash_cmd (osh, cc, SFLASH_ST_RDSR);
-      return R_REG (osh, &cc->flashdata) & SFLASH_ST_WIP;
-    case SFLASH_AT:
-      /* Check for Atmel Ready bit */
-      sflash_cmd (osh, cc, SFLASH_AT_STATUS);
-      return !(R_REG (osh, &cc->flashdata) & SFLASH_AT_READY);
-    }
+	/* Call erase callback */
+	if (erase->callback)
+		erase->callback(erase);
 
-  return 0;
+	return ret;
 }
 
-/* Write len bytes starting at offset into buf. Returns number of bytes
- * written. Caller should poll for completion.
- */
-int
-sflash_write (sb_t * sbh, chipcregs_t * cc, uint offset, uint len,
-	      const uchar * buf)
+#if LINUX_VERSION_CODE < 0x20212 && defined(MODULE)
+#define sflash_mtd_init init_module
+#define sflash_mtd_exit cleanup_module
+#endif
+
+mod_init_t
+sflash_mtd_init(void)
 {
-  struct sflash *sfl;
-  int ret = 0;
-  bool is4712b0;
-  uint32 page, byte, mask;
-  osl_t *osh;
-
-  ASSERT (sbh);
-
-  osh = sb_osh (sbh);
-
-  if (!len)
-    return 0;
-
-  if ((offset + len) > sflash.size)
-    return -22;
-
-  sfl = &sflash;
-  switch (sfl->type)
-    {
-    case SFLASH_ST:
-      is4712b0 = (sbh->chip == BCM4712_CHIP_ID) && (sbh->chiprev == 3);
-      /* Enable writes */
-      sflash_cmd (osh, cc, SFLASH_ST_WREN);
-      if (is4712b0)
-	{
-	  mask = 1 << 14;
-	  W_REG (osh, &cc->flashaddress, offset);
-	  W_REG (osh, &cc->flashdata, *buf++);
-	  /* Set chip select */
-	  OR_REG (osh, &cc->gpioout, mask);
-	  /* Issue a page program with the first byte */
-	  sflash_cmd (osh, cc, SFLASH_ST_PP);
-	  ret = 1;
-	  offset++;
-	  len--;
-	  while (len > 0)
-	    {
-	      if ((offset & 255) == 0)
-		{
-		  /* Page boundary, drop cs and return */
-		  AND_REG (osh, &cc->gpioout, ~mask);
-		  if (!sflash_poll (sbh, cc, offset))
-		    {
-		      /* Flash rejected command */
-		      return -11;
-		    }
-		  return ret;
-		}
-	      else
-		{
-		  /* Write single byte */
-		  sflash_cmd (osh, cc, *buf++);
-		}
-	      ret++;
-	      offset++;
-	      len--;
-	    }
-	  /* All done, drop cs if needed */
-	  if ((offset & 255) != 1)
-	    {
-	      /* Drop cs */
-	      AND_REG (osh, &cc->gpioout, ~mask);
-	      if (!sflash_poll (sbh, cc, offset))
-		{
-		  /* Flash rejected command */
-		  return -12;
-		}
-	    }
-	}
-      else if (sbh->ccrev >= 20)
-	{
-	  W_REG (NULL, &cc->flashaddress, offset);
-	  W_REG (NULL, &cc->flashdata, *buf++);
-	  /* Issue a page program with CSA bit set */
-	  sflash_cmd (osh, cc, SFLASH_ST_CSA | SFLASH_ST_PP);
-	  ret = 1;
-	  offset++;
-	  len--;
-	  while (len > 0)
-	    {
-	      if ((offset & 255) == 0)
-		{
-		  /* Page boundary, poll droping cs and return */
-		  W_REG (NULL, &cc->flashcontrol, 0);
-		  if (!sflash_poll (sbh, cc, offset))
-		    {
-		      /* Flash rejected command */
-		      return -11;
-		    }
-		  return ret;
-		}
-	      else
-		{
-		  /* Write single byte */
-		  sflash_cmd (osh, cc, SFLASH_ST_CSA | *buf++);
-		}
-	      ret++;
-	      offset++;
-	      len--;
-	    }
-	  /* All done, drop cs if needed */
-	  if ((offset & 255) != 1)
-	    {
-	      /* Drop cs, poll */
-	      W_REG (NULL, &cc->flashcontrol, 0);
-	      if (!sflash_poll (sbh, cc, offset))
-		{
-		  /* Flash rejected command */
-		  return -12;
-		}
-	    }
-	}
-      else
-	{
-	  ret = 1;
-	  W_REG (osh, &cc->flashaddress, offset);
-	  W_REG (osh, &cc->flashdata, *buf);
-	  /* Page program */
-	  sflash_cmd (osh, cc, SFLASH_ST_PP);
-	}
-      break;
-    case SFLASH_AT:
-      mask = sfl->blocksize - 1;
-      page = (offset & ~mask) << 1;
-      byte = offset & mask;
-      /* Read main memory page into buffer 1 */
-      if (byte || (len < sfl->blocksize))
-	{
-	  W_REG (osh, &cc->flashaddress, page);
-	  sflash_cmd (osh, cc, SFLASH_AT_BUF1_LOAD);
-	  /* 250 us for AT45DB321B */
-	  SPINWAIT (sflash_poll (sbh, cc, offset), 1000);
-	  ASSERT (!sflash_poll (sbh, cc, offset));
+	struct pci_dev *pdev;
+	int ret = 0;
+	struct sflash *info;
+	uint i;
+#ifdef CONFIG_MTD_PARTITIONS
+	struct mtd_partition *parts;
+#endif
+
+	if (!(pdev = pci_find_device(VENDOR_BROADCOM, SB_CC, NULL))) {
+		printk(KERN_ERR "sflash: chipcommon not found\n");
+		return -ENODEV;
 	}
-      /* Write into buffer 1 */
-      for (ret = 0; (ret < (int) len) && (byte < sfl->blocksize); ret++)
-	{
-	  W_REG (osh, &cc->flashaddress, byte++);
-	  W_REG (osh, &cc->flashdata, *buf++);
-	  sflash_cmd (osh, cc, SFLASH_AT_BUF1_WRITE);
-	}
-      /* Write buffer 1 into main memory page */
-      W_REG (osh, &cc->flashaddress, page);
-      sflash_cmd (osh, cc, SFLASH_AT_BUF1_PROGRAM);
-      break;
-    }
-
-  return ret;
-}
 
-/* Erase a region. Returns number of bytes scheduled for erasure.
- * Caller should poll for completion.
- */
-int
-sflash_erase (sb_t * sbh, chipcregs_t * cc, uint offset)
-{
-  struct sflash *sfl;
-  osl_t *osh;
-
-  ASSERT (sbh);
-
-  osh = sb_osh (sbh);
-
-  if (offset >= sflash.size)
-    return -22;
-
-  sfl = &sflash;
-  switch (sfl->type)
-    {
-    case SFLASH_ST:
-      sflash_cmd (osh, cc, SFLASH_ST_WREN);
-      W_REG (osh, &cc->flashaddress, offset);
-      sflash_cmd (osh, cc, SFLASH_ST_SE);
-      return sfl->blocksize;
-    case SFLASH_AT:
-      W_REG (osh, &cc->flashaddress, offset << 1);
-      sflash_cmd (osh, cc, SFLASH_AT_PAGE_ERASE);
-      return sfl->blocksize;
-    }
-
-  return 0;
-}
+	memset(&sflash, 0, sizeof(struct sflash_mtd));
+	init_MUTEX(&sflash.lock);
 
-/*
- * writes the appropriate range of flash, a NULL buf simply erases
- * the region of flash
- */
-int
-sflash_commit (sb_t * sbh, chipcregs_t * cc, uint offset, uint len,
-	       const uchar * buf)
-{
-  struct sflash *sfl;
-  uchar *block = NULL, *cur_ptr, *blk_ptr;
-  uint blocksize = 0, mask, cur_offset, cur_length, cur_retlen, remainder;
-  uint blk_offset, blk_len, copied;
-  int bytes, ret = 0;
-  osl_t *osh;
-
-  ASSERT (sbh);
-
-  osh = sb_osh (sbh);
-
-  /* Check address range */
-  if (len <= 0)
-    return 0;
-
-  sfl = &sflash;
-  if ((offset + len) > sfl->size)
-    return -1;
-
-  blocksize = sfl->blocksize;
-  mask = blocksize - 1;
-
-  /* Allocate a block of mem */
-  if (!(block = MALLOC (osh, blocksize)))
-    return -1;
-
-  while (len)
-    {
-      /* Align offset */
-      cur_offset = offset & ~mask;
-      cur_length = blocksize;
-      cur_ptr = block;
-
-      remainder = blocksize - (offset & mask);
-      if (len < remainder)
-	cur_retlen = len;
-      else
-	cur_retlen = remainder;
-
-      /* buf == NULL means erase only */
-      if (buf)
-	{
-	  /* Copy existing data into holding block if necessary */
-	  if ((offset & mask) || (len < blocksize))
-	    {
-	      blk_offset = cur_offset;
-	      blk_len = cur_length;
-	      blk_ptr = cur_ptr;
-
-	      /* Copy entire block */
-	      while (blk_len)
-		{
-		  copied =
-		    sflash_read (sbh, cc, blk_offset, blk_len, blk_ptr);
-		  blk_offset += copied;
-		  blk_len -= copied;
-		  blk_ptr += copied;
-		}
-	    }
+	/* attach to the backplane */
+	if (!(sflash.sbh = sb_kattach(SB_OSH))) {
+		printk(KERN_ERR "sflash: error attaching to backplane\n");
+		ret = -EIO;
+		goto fail;
+	}
 
-	  /* Copy input data into holding block */
-	  memcpy (cur_ptr + (offset & mask), buf, cur_retlen);
+	/* Map registers and flash base */
+	if (!(sflash.cc = ioremap_nocache(pci_resource_start(pdev, 0),
+					  pci_resource_len(pdev, 0)))) {
+		printk(KERN_ERR "sflash: error mapping registers\n");
+		ret = -EIO;
+		goto fail;
 	}
 
-      /* Erase block */
-      if ((ret = sflash_erase (sbh, cc, (uint) cur_offset)) < 0)
-	goto done;
-      while (sflash_poll (sbh, cc, (uint) cur_offset));
-
-      /* buf == NULL means erase only */
-      if (!buf)
-	{
-	  offset += cur_retlen;
-	  len -= cur_retlen;
-	  continue;
+	/* Initialize serial flash access */
+	if (!(info = sflash_init(sflash.sbh, sflash.cc))) {
+		printk(KERN_ERR "sflash: found no supported devices\n");
+		ret = -ENODEV;
+		goto fail;
 	}
 
-      /* Write holding block */
-      while (cur_length > 0)
-	{
-	  if ((bytes = sflash_write (sbh, cc,
-				     (uint) cur_offset,
-				     (uint) cur_length,
-				     (uchar *) cur_ptr)) < 0)
-	    {
-	      ret = bytes;
-	      goto done;
-	    }
-	  while (sflash_poll (sbh, cc, (uint) cur_offset));
-	  cur_offset += bytes;
-	  cur_length -= bytes;
-	  cur_ptr += bytes;
+	printk(KERN_INFO "sflash: found serial flash; blocksize=%dKB, numblocks=%d, size=%dKB\n",info->blocksize/1024,info->numblocks,info->size/1024);
+
+	/* Setup region info */
+	sflash.region.offset = 0;
+	sflash.region.erasesize = info->blocksize;
+	sflash.region.numblocks = info->numblocks;
+	if (sflash.region.erasesize > sflash.mtd.erasesize)
+		sflash.mtd.erasesize = sflash.region.erasesize;
+	sflash.mtd.size = info->size;
+	sflash.mtd.numeraseregions = 1;
+
+	/* Register with MTD */
+	sflash.mtd.name = "sflash";
+	sflash.mtd.type = MTD_NORFLASH;
+	sflash.mtd.flags = MTD_CAP_NORFLASH;
+	sflash.mtd.eraseregions = &sflash.region;
+	sflash.mtd.module = THIS_MODULE;
+	sflash.mtd.erase = sflash_mtd_erase;
+	sflash.mtd.read = sflash_mtd_read;
+	sflash.mtd.write = sflash_mtd_write;
+	sflash.mtd.priv = &sflash;
+
+#ifdef CONFIG_MTD_PARTITIONS
+	parts = init_mtd_partitions(&sflash.mtd, sflash.mtd.size);
+	for (i = 0; parts[i].name; i++);
+	ret = add_mtd_partitions(&sflash.mtd, parts, i);
+#else
+	ret = add_mtd_device(&sflash.mtd);
+#endif
+	if (ret) {
+		printk(KERN_ERR "sflash: add_mtd failed\n");
+		goto fail;
 	}
 
-      offset += cur_retlen;
-      len -= cur_retlen;
-      buf += cur_retlen;
-    }
+	return 0;
+
+ fail:
+	if (sflash.cc)
+		iounmap((void *) sflash.cc);
+	if (sflash.sbh)
+		sb_detach(sflash.sbh);
+	return ret;
+}
 
-  ret = len;
-done:
-  if (block)
-    MFREE (osh, block, blocksize);
-  return ret;
+mod_exit_t
+sflash_mtd_exit(void)
+{
+#ifdef CONFIG_MTD_PARTITIONS
+	del_mtd_partitions(&sflash.mtd);
+#else
+	del_mtd_device(&sflash.mtd);
+#endif
+	iounmap((void *) sflash.cc);
+	sb_detach(sflash.sbh);
 }
+
+module_init(sflash_mtd_init);
+module_exit(sflash_mtd_exit);