update brcm-2.4 to 2.4.35.4, integrate new broadcom system code, update broadcom-wl to a contributed version (v4.150.10.5) - no bcm57xx support yet, will follow shortly

SVN-Revision: 10137

2 files changed, 484 insertions, 252 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 a987388ab2..62c7802221 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,298 +1,531 @@
 /*
  * Broadcom SiliconBackplane chipcommon serial flash interface
  *
- * Copyright 2001-2003, 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 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.
  *
- * $Id: sflash.c,v 1.1.1.3 2003/11/10 17:43:38 hyin Exp $
+ * $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>
-
-#ifdef CONFIG_MTD_PARTITIONS
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/partitions.h>
-#include <linux/minix_fs.h>
-#include <linux/ext2_fs.h>
-#include <linux/romfs_fs.h>
-#include <linux/cramfs_fs.h>
-#include <linux/jffs2.h>
-#endif
-
 #include <typedefs.h>
-#include <bcmdevs.h>
-#include <bcmutils.h>
 #include <osl.h>
-#include <bcmutils.h>
-#include <bcmnvram.h>
+#include <sbutils.h>
 #include <sbconfig.h>
 #include <sbchipc.h>
+#include <bcmdevs.h>
 #include <sflash.h>
-#include <trxhdr.h>
-
-#ifdef CONFIG_MTD_PARTITIONS
-extern struct mtd_partition * init_mtd_partitions(struct mtd_info *mtd, size_t size);
-#endif
-
-struct sflash_mtd {
-	chipcregs_t *cc;
-	struct semaphore lock;
-	struct mtd_info mtd;
-	struct mtd_erase_region_info regions[1];
-};
 
 /* Private global state */
-static struct sflash_mtd sflash;
+static struct sflash sflash;
 
-static int
-sflash_mtd_poll(struct sflash_mtd *sflash, unsigned int offset, int timeout)
+/* Issue a serial flash command */
+static INLINE void
+sflash_cmd (osl_t * osh, chipcregs_t * cc, uint opcode)
 {
-	int now = jiffies;
-	int ret = 0;
+  W_REG (osh, &cc->flashcontrol, SFLASH_START | opcode);
+  while (R_REG (osh, &cc->flashcontrol) & SFLASH_BUSY);
+}
 
-	for (;;) {
-		if (!sflash_poll(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);
+/* 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;
+	}
+      break;
+    }
 
-	return ret;
+  sflash.size = sflash.blocksize * sflash.numblocks;
+  return sflash.size ? &sflash : NULL;
 }
 
-static int
-sflash_mtd_read(struct mtd_info *mtd, loff_t from, size_t len, size_t *retlen, u_char *buf)
+/* 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)
 {
-	struct sflash_mtd *sflash = (struct sflash_mtd *) mtd->priv;
-	int bytes, ret = 0;
-
-	/* Check address range */
-	if (!len)
-		return 0;
-	if ((from + len) > mtd->size)
-		return -EINVAL;
-	
-	down(&sflash->lock);
-
-	*retlen = 0;
-	while (len) {
-		if ((bytes = sflash_read(sflash->cc, (uint) from, len, buf)) < 0) {
-			ret = bytes;
-			break;
-		}
-		from += (loff_t) bytes;
-		len -= bytes;
-		buf += bytes;
-		*retlen += bytes;
-	}
+  uint8 *from, *to;
+  int cnt, i;
+  osl_t *osh;
 
-	up(&sflash->lock);
+  ASSERT (sbh);
 
-	return ret;
-}
+  if (!len)
+    return 0;
 
-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)
-		return 0;
-	if ((to + len) > mtd->size)
-		return -EINVAL;
-
-	down(&sflash->lock);
-
-	*retlen = 0;
-	while (len) {
-		if ((bytes = sflash_write(sflash->cc, (uint) to, 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;
-	}
+  if ((offset + len) > sflash.size)
+    return -22;
+
+  if ((len >= 4) && (offset & 3))
+    cnt = 4 - (offset & 3);
+  else if ((len >= 4) && ((uintptr) buf & 3))
+    cnt = 4 - ((uintptr) buf & 3);
+  else
+    cnt = len;
+
+  osh = sb_osh (sbh);
 
-	up(&sflash->lock);
+  from = (uint8 *) (uintptr) OSL_UNCACHED (SB_FLASH2 + offset);
+  to = (uint8 *) buf;
 
-	return ret;
+  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);
 }
 
-static int
-sflash_mtd_erase(struct mtd_info *mtd, struct erase_info *erase)
+/* Poll for command completion. Returns zero when complete. */
+int
+sflash_poll (sb_t * sbh, chipcregs_t * cc, uint offset)
 {
-	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->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;
-	}
+  osl_t *osh;
 
-	up(&sflash->lock);
+  ASSERT (sbh);
 
-	/* Set erase status */
-	if (ret)
-		erase->state = MTD_ERASE_FAILED;
-	else 
-		erase->state = MTD_ERASE_DONE;
+  osh = sb_osh (sbh);
 
-	/* Call erase callback */
-	if (erase->callback)
-		erase->callback(erase);
+  if (offset >= sflash.size)
+    return -22;
 
-	return ret;
-}
+  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);
+    }
 
-#if LINUX_VERSION_CODE < 0x20212 && defined(MODULE)
-#define sflash_mtd_init init_module
-#define sflash_mtd_exit cleanup_module
-#endif
+  return 0;
+}
 
-mod_init_t
-sflash_mtd_init(void)
+/* 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)
 {
-	struct pci_dev *pdev;
-	int ret = 0;
-	struct sflash *info;
-	uint bank, 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;
+  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));
 	}
+      /* 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;
+    }
 
-	memset(&sflash, 0, sizeof(struct sflash_mtd));
-	init_MUTEX(&sflash.lock);
+  return ret;
+}
 
-	/* 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 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;
+}
 
-	/* Initialize serial flash access */
-	info = sflash_init(sflash.cc);
+/*
+ * 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;
+		}
+	    }
 
-	if (!info) {
-		printk(KERN_ERR "sflash: found no supported devices\n");
-		ret = -ENODEV;
-		goto fail;
+	  /* Copy input data into holding block */
+	  memcpy (cur_ptr + (offset & mask), buf, cur_retlen);
 	}
 
-	/* Setup banks */
-	sflash.regions[0].offset = 0;
-	sflash.regions[0].erasesize = info->blocksize;
-	sflash.regions[0].numblocks = info->numblocks;
-	if (sflash.regions[0].erasesize > sflash.mtd.erasesize)
-		sflash.mtd.erasesize = sflash.regions[0].erasesize;
-	if (sflash.regions[0].erasesize * sflash.regions[0].numblocks) {
-		sflash.mtd.size += sflash.regions[0].erasesize * sflash.regions[0].numblocks;
-	}
-	sflash.mtd.numeraseregions = 1;
-	ASSERT(sflash.mtd.size == info->size);
-
-	/* Register with MTD */
-	sflash.mtd.name = "sflash";
-	sflash.mtd.type = MTD_NORFLASH;
-	sflash.mtd.flags = MTD_CAP_NORFLASH;
-	sflash.mtd.eraseregions = sflash.regions;
-	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;
+      /* 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;
 	}
 
-	return 0;
+      /* 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;
+	}
 
- fail:
-	if (sflash.cc)
-		iounmap((void *) sflash.cc);
-	return ret;
-}
+      offset += cur_retlen;
+      len -= cur_retlen;
+      buf += cur_retlen;
+    }
 
-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);
+  ret = len;
+done:
+  if (block)
+    MFREE (osh, block, blocksize);
+  return ret;
 }
-
-module_init(sflash_mtd_init);
-module_exit(sflash_mtd_exit);
diff --git a/target/linux/brcm-2.4/files/drivers/mtd/maps/bcm947xx-flash.c b/target/linux/brcm-2.4/files/drivers/mtd/maps/bcm947xx-flash.c
index 1641784681..eea7f137d7 100644
--- a/target/linux/brcm-2.4/files/drivers/mtd/maps/bcm947xx-flash.c
+++ b/target/linux/brcm-2.4/files/drivers/mtd/maps/bcm947xx-flash.c
@@ -56,7 +56,6 @@
 #include <typedefs.h>
 #include <osl.h>
 #include <bcmnvram.h>
-#include <bcmutils.h>
 #include <sbconfig.h>
 #include <sbchipc.h>
 #include <sbutils.h>
@@ -470,7 +469,7 @@ mod_init_t init_bcm947xx_map(void)
 
 	/* Check strapping option if chipcommon exists */
 	if ((cc = sb_setcore(sbh, SB_CC, 0))) {
-		fltype = readl(&cc->capabilities) & CAP_FLASH_MASK;
+		fltype = readl(&cc->capabilities) & CC_CAP_FLASH_MASK;
 		if (fltype == PFLASH) {
 			bcm947xx_map.map_priv_2 = 1;
 			window_addr = 0x1c000000;