drivers/mtd/ralink/ralink_spi.c

/*
 * MTD SPI driver for ST M25Pxx flash chips
 *
 * Author: Mike Lavender, mike@steroidmicros.com
 *
 * Copyright (c) 2005, Intec Automation Inc.
 * Copyright (c) 2012, CradlePoint Technology, Inc.
 *
 * Some parts are based on lart.c by Abraham Van Der Merwe
 *
 * Cleaned up and generalized based on mtd_dataflash.c
 *
 * Ralink SPI updates and adjustments by CradlePoint
 *
 * This code is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 */
#include <linux/module.h>
#include <linux/types.h>
#include <linux/kernel.h>
#include <linux/init.h>
#include <linux/sched.h>
#include <linux/slab.h>
#include <linux/device.h>
#include <linux/platform_device.h>
#include <linux/mtd/mtd.h>
#include <linux/mtd/map.h>
#include <linux/mtd/partitions.h>
#include <linux/mtd/physmap.h>
#include <linux/reboot.h>
#include <linux/semaphore.h>
#include <linux/delay.h>
#include <linux/err.h>

#include "../maps/ralink-flash.h"

#if defined (CONFIG_RALINK_RT6855A)
#include "bbu_spiflash.h"
#else
#include "ralink_spi.h"
#endif

/* Choose the SPI flash mode */
#if defined (CONFIG_RALINK_RT6855A)
#define BBU_MODE		// BBU SPI flash controller
#define MORE_BUF_MODE
#else
#define USER_MODE		// SPI flash user mode support by default
//#define COMMAND_MODE		// SPI flash command mode support
#endif

#if !defined USER_MODE && !defined COMMAND_MODE && !defined BBU_MODE
#error "Please choose the correct mode of SPI flash controller"
#endif

/******************************************************************************
 * SPI FLASH elementray definition and function
 ******************************************************************************/

#define FLASH_PAGESIZE		256
#define MX_4B_MODE			/* MXIC 4 Byte Mode */

/* Flash opcodes. */
#define OPCODE_WREN		6	/* Write enable */
#define OPCODE_WRDI		4	/* Write disable */
#define OPCODE_RDSR		5	/* Read status register */
#define OPCODE_WRSR		1	/* Write status register */
#define OPCODE_READ		3	/* Read data bytes */
#define OPCODE_PP		2	/* Page program */
#define OPCODE_SE		0xD8	/* Sector erase */
#define OPCODE_RES		0xAB	/* Read Electronic Signature */
#define OPCODE_RDID		0x9F	/* Read JEDEC ID */

#define OPCODE_FAST_READ	0x0B		/* Fast Read */
#define OPCODE_DOR			0x3B	/* Dual Output Read */
#define OPCODE_QOR			0x6B	/* Quad Output Read */
#define OPCODE_DIOR			0xBB	/* Dual IO High Performance Read */
#define OPCODE_QIOR			0xEB	/* Quad IO High Performance Read */
#define OPCODE_READ_ID		0x90	/* Read Manufacturer and Device ID */

#define OPCODE_P4E			0x20	/* 4KB Parameter Sectore Erase */
#define OPCODE_P8E			0x40	/* 8KB Parameter Sectore Erase */
#define OPCODE_BE			0x60	/* Bulk Erase */
#define OPCODE_BE1			0xC7	/* Bulk Erase */
#define OPCODE_QPP			0x32	/* Quad Page Programing */

/* Status Register bits. */
#define SR_WIP			1	/* Write in progress */
#define SR_WEL			2	/* Write enable latch */
#define SR_BP0			4	/* Block protect 0 */
#define SR_BP1			8	/* Block protect 1 */
#define SR_BP2			0x10	/* Block protect 2 */
#define SR_EPE			0x20	/* Erase/Program error */
#define SR_SRWD			0x80	/* SR write protect */

/* Security Register bits. */
#define    READ_SCUR      0x2b    /* Read Security register */
#define    SECR_OTP       0x01
#define    SECR_LDSO      0x02
#define    SECR_4BYTE     0x04
#define    SECR_CP        0x10
#define    SECR_PFAIL     0x20
#define    SECR_EFAIL     0x40
#define    SECR_WPSEL     0x80


/*
  The MTD partition map and number of partitions.
*/
static struct mtd_partition *rt2880_partitions;
static int nr_parts;  

//#define SPI_DEBUG
#if !defined (SPI_DEBUG)

#define ra_inl(addr)  (*(volatile unsigned int *)(addr))
#define ra_outl(addr, value)  (*(volatile unsigned int *)(addr) = (value))
#define ra_dbg(args...) do {} while(0)
//#define ra_dbg(args...) do { printk(args); } while(0)

#else

#define ra_dbg(args...) do { printk(args); } while(0)
#define _ra_inl(addr)  (*(volatile unsigned int *)(addr))
#define _ra_outl(addr, value)  (*(volatile unsigned int *)(addr) = (value))

u32 ra_inl(u32 addr)
{	
	u32 retval = _ra_inl(addr);
	
	printk("%s(%x) => %x \n", __func__, addr, retval);

	return retval;	
}

u32 ra_outl(u32 addr, u32 val)
{
	_ra_outl(addr, val);

	printk("%s(%x, %x) \n", __func__, addr, val);

	return val;	
}

#endif // SPI_DEBUG //

#define ra_aor(addr, a_mask, o_value)  ra_outl(addr, (ra_inl(addr) & (a_mask)) | (o_value))
#define ra_and(addr, a_mask)  ra_aor(addr, a_mask, 0)
#define ra_or(addr, o_value)  ra_aor(addr, -1, o_value)

#define SPIC_READ_BYTES (1<<0)
#define SPIC_WRITE_BYTES (1<<1)

#if 1
void usleep(unsigned int usecs)
{
        unsigned long timeout = usecs_to_jiffies(usecs);

        while (timeout)
                timeout = schedule_timeout_interruptible(timeout);
}
#endif


#if defined USER_MODE || defined COMMAND_MODE
static unsigned int spi_wait_nsec = 0;
static int spic_busy_wait(void)
{
	do {
		if ((ra_inl(RT2880_SPISTAT_REG) & 0x01) == 0)
			return 0;
	} while (spi_wait_nsec >> 1);

	printk("%s: fail \n", __func__);
	return -1;
}
#elif defined BBU_MODE
static int bbu_spic_busy_wait(void)
{
	int n = 100000;
	do {
		if ((ra_inl(SPI_REG_CTL) & SPI_CTL_BUSY) == 0)
			return 0;
		udelay(1);
	} while (--n > 0);

	printk("%s: fail \n", __func__);
	return -1;
}
#endif // USER_MODE || COMMAND_MODE //


#ifdef USER_MODE
/*
 * @cmd: command and address
 * @n_cmd: size of command, in bytes
 * @buf: buffer into which data will be read/written
 * @n_buf: size of buffer, in bytes
 * @flag: tag as READ/WRITE
 *
 * @return: if write_onlu, -1 means write fail, or return writing counter.
 * @return: if read, -1 means read fail, or return reading counter.
 */
static int spic_transfer(const u8 *cmd, int n_cmd, u8 *buf, int n_buf, int flag)
{
	int retval = -1;
	ra_dbg("cmd(%x): %x %x %x %x , buf:%x len:%x, flag:%s \n",
			n_cmd, cmd[0], cmd[1], cmd[2], cmd[3],
			(buf)? (*buf) : 0, n_buf,
			(flag == SPIC_READ_BYTES)? "read" : "write");

	// assert CS and we are already CLK normal high
	ra_and(RT2880_SPICTL_REG, ~(SPICTL_SPIENA_HIGH));

	// write command
	for (retval = 0; retval < n_cmd; retval++) {
		ra_outl(RT2880_SPIDATA_REG, cmd[retval]);
		ra_or(RT2880_SPICTL_REG, SPICTL_STARTWR);
		if (spic_busy_wait()) {
			retval = -1;
			goto end_trans;
		}
	}

	// read / write  data
	if (flag & SPIC_READ_BYTES) {
		for (retval = 0; retval < n_buf; retval++) {
			ra_or(RT2880_SPICTL_REG, SPICTL_STARTRD);
			if (spic_busy_wait())
				goto end_trans;
			buf[retval] = (u8) ra_inl(RT2880_SPIDATA_REG);
		}

	}
	else if (flag & SPIC_WRITE_BYTES) {
		for (retval = 0; retval < n_buf; retval++) {
			ra_outl(RT2880_SPIDATA_REG, buf[retval]);
			ra_or(RT2880_SPICTL_REG, SPICTL_STARTWR);
			if (spic_busy_wait())
				goto end_trans;
		}
	}

end_trans:
	// de-assert CS and
	ra_or (RT2880_SPICTL_REG, (SPICTL_SPIENA_HIGH));

	return retval;
}

static int spic_read(const u8 *cmd, size_t n_cmd, u8 *rxbuf, size_t n_rx)
{
	return spic_transfer(cmd, n_cmd, rxbuf, n_rx, SPIC_READ_BYTES);
}

static int spic_write(const u8 *cmd, size_t n_cmd, const u8 *txbuf, size_t n_tx)
{
	return spic_transfer(cmd, n_cmd, (u8 *)txbuf, n_tx, SPIC_WRITE_BYTES);
}
#endif // USER_MODE //

void spic_init(void)
{
#if defined USER_MODE || defined COMMAND_MODE

	// use normal(SPI) mode instead of GPIO mode
	//ra_and(RALINK_REG_GPIOMODE, ~(1 << 1));

	// reset spi block
	ra_or(RT2880_RSTCTRL_REG, RSTCTRL_SPI_RESET);
	udelay(1);
	ra_and(RT2880_RSTCTRL_REG, ~RSTCTRL_SPI_RESET);

	// FIXME, clk_div should depend on spi-flash. 
	// mode 0 (SPICLKPOL = 0) & (RXCLKEDGE_FALLING = 0)
	// mode 3 (SPICLKPOL = 1) & (RXCLKEDGE_FALLING = 0)
	ra_outl(RT2880_SPICFG_REG, SPICFG_MSBFIRST | SPICFG_TXCLKEDGE_FALLING | CFG_CLK_DIV | SPICFG_SPICLKPOL );

	// set idle state
	ra_outl(RT2880_SPICTL_REG, SPICTL_HIZSDO | SPICTL_SPIENA_HIGH);

	spi_wait_nsec = (8 * 1000 / (128 / CFG_CLK_DIV) ) >> 1 ;
	//printk("spi_wait_nsec: %x \n", spi_wait_nsec);
#elif defined BBU_MODE
	// enable SMC bank 0 alias addressing
	ra_or(RALINK_SYSCTL_BASE + 0x38, 0x80000000);
#endif
}


struct chip_info {
	char		*name;
	u8		id;
	u32		jedec_id;
	unsigned long	sector_size;
	unsigned int	n_sectors;
	char		addr4b;
};

static struct chip_info chips_data [] = {
	/* REVISIT: fill in JEDEC ids, for parts that have them */
	{ "AT25DF321",		0x1f, 0x47000000, 64 * 1024, 64,  0 },
	{ "AT26DF161",		0x1f, 0x46000000, 64 * 1024, 32,  0 },
	{ "FL016AIF",		0x01, 0x02140000, 64 * 1024, 32,  0 },
	{ "FL064AIF",		0x01, 0x02160000, 64 * 1024, 128, 0 },
	{ "MX25L1605D",		0xc2, 0x2015c220, 64 * 1024, 32,  0 },
	{ "MX25L3205D",		0xc2, 0x2016c220, 64 * 1024, 64,  0 },
	{ "MX25L6405D",		0xc2, 0x2017c220, 64 * 1024, 128, 0 },
	{ "MX25L12805D",	0xc2, 0x2018c220, 64 * 1024, 256, 0 },
#ifdef MX_4B_MODE 
	{ "MX25L25635E",	0xc2, 0x2019c220, 64 * 1024, 512, 1 },
#endif
	{ "S25FL128P",		0x01, 0x20180301, 64 * 1024, 256, 0 },
	{ "S25FL129P",		0x01, 0x20184D01, 64 * 1024, 256, 0 },
	{ "S25FL032P",		0x01, 0x02154D00, 64 * 1024, 64,  0 },
	{ "S25FL064P",		0x01, 0x02164D00, 64 * 1024, 128, 0 },
	{ "EN25F16",		0x1c, 0x31151c31, 64 * 1024, 32,  0 },
	{ "EN25F32",		0x1c, 0x31161c31, 64 * 1024, 64,  0 },
	{ "EN25Q32",		0x1c, 0x30161c30, 64 * 1024, 64,  0 }, //EN25Q32B
	{ "EN25F64",		0x1c, 0x20171c20, 64 * 1024, 128, 0 }, //EN25P64
	{ "EN25Q64",		0x1c, 0x30171c30, 64 * 1024, 128, 0 },
	{ "W25Q32BV",		0xef, 0x30160000, 64 * 1024, 64,  0 },
	{ "W25Q32BV",		0xef, 0x40160000, 64 * 1024, 64,  0 },
	{ "W25Q64BV",		0xef, 0x40170000, 64 * 1024, 128, 0 }, //S25FL064K
	{ "W25Q128BV",          0xef, 0x40180000, 64 * 1024, 256, 0 },
};


struct flash_info {
	struct semaphore	lock;
	struct mtd_info		mtd;
	struct chip_info	*chip;
	u8			command[5];
};

struct flash_info *flash = NULL;


#ifdef BBU_MODE
#ifdef MORE_BUF_MODE
static int bbu_mb_spic_trans(const u8 code, const u32 addr, u8 *buf, const size_t n_tx, const size_t n_rx, int flag)
{
	u32 reg;
	int i, q, r;
	int rc = -1;

	if (flag != SPIC_READ_BYTES && flag != SPIC_WRITE_BYTES) {
		printk("we currently support more-byte-mode for reading and writing data only\n");
		return -1;
	}

	/* step 0. enable more byte mode */
	ra_or(SPI_REG_MASTER, (1 << 2));

	bbu_spic_busy_wait();

	/* step 1. set opcode & address, and fix cmd bit count to 32 (or 40) */
#ifdef MX_4B_MODE 
	if (flash && flash->chip->addr4b) {
		ra_and(SPI_REG_CTL, ~SPI_CTL_ADDREXT_MASK);
		ra_or(SPI_REG_CTL, (code << 24) & SPI_CTL_ADDREXT_MASK);
		ra_outl(SPI_REG_OPCODE, addr);
	}
	else
#endif
	{
		ra_outl(SPI_REG_OPCODE, (code << 24) & 0xff000000);
		ra_or(SPI_REG_OPCODE, (addr & 0xffffff));
	}
	ra_and(SPI_REG_MOREBUF, ~SPI_MBCTL_CMD_MASK);
#ifdef MX_4B_MODE 
	if (flash && flash->chip->addr4b)
		ra_or(SPI_REG_MOREBUF, (40 << 24));
	else
#endif
		ra_or(SPI_REG_MOREBUF, (32 << 24));

	/* step 2. write DI/DO data #0 ~ #7 */
	if (flag & SPIC_WRITE_BYTES) {
		if (buf == NULL) {
			printk("%s: write null buf\n", __func__);
			goto RET_MB_TRANS;
		}
		for (i = 0; i < n_tx; i++) {
			q = i / 4;
			r = i % 4;
			if (r == 0)
				ra_outl(SPI_REG_DATA(q), 0);
			ra_or(SPI_REG_DATA(q), (*(buf + i) << (r * 8)));
		}
	}

	/* step 3. set rx (miso_bit_cnt) and tx (mosi_bit_cnt) bit count */
	ra_and(SPI_REG_MOREBUF, ~SPI_MBCTL_TX_RX_CNT_MASK);
	ra_or(SPI_REG_MOREBUF, (n_rx << 3 << 12));
	ra_or(SPI_REG_MOREBUF, n_tx << 3);

	/* step 4. kick */
	ra_or(SPI_REG_CTL, SPI_CTL_START);

	/* step 5. wait spi_master_busy */
	bbu_spic_busy_wait();
	if (flag & SPIC_WRITE_BYTES) {
		rc = 0;
		goto RET_MB_TRANS;
	}

	/* step 6. read DI/DO data #0 */
	if (flag & SPIC_READ_BYTES) {
		if (buf == NULL) {
			printk("%s: read null buf\n", __func__);
			return -1;
		}
		for (i = 0; i < n_rx; i++) {
			q = i / 4;
			r = i % 4;
			reg = ra_inl(SPI_REG_DATA(q));
			*(buf + i) = (u8)(reg >> (r * 8));
		}
	}

	rc = 0;
RET_MB_TRANS:
	/* step #. disable more byte mode */
	ra_and(SPI_REG_MASTER, ~(1 << 2));
	return rc;
}
#endif // MORE_BUF_MODE //

static int bbu_spic_trans(const u8 code, const u32 addr, u8 *buf, const size_t n_tx, const size_t n_rx, int flag)
{
	u32 reg;

	bbu_spic_busy_wait();

	/* step 1. set opcode & address */
#ifdef MX_4B_MODE 
	if (flash && flash->chip->addr4b) {
		ra_and(SPI_REG_CTL, ~SPI_CTL_ADDREXT_MASK);
		ra_or(SPI_REG_CTL, addr & SPI_CTL_ADDREXT_MASK);
	}
#endif
	ra_outl(SPI_REG_OPCODE, ((addr & 0xffffff) << 8));
	ra_or(SPI_REG_OPCODE, code);

	/* step 2. write DI/DO data #0 */
	if (flag & SPIC_WRITE_BYTES) {
		if (buf == NULL) {
			printk("%s: write null buf\n", __func__);
			return -1;
		}
		ra_outl(SPI_REG_DATA0, 0);
		switch (n_tx) {
		case 8:
			ra_or(SPI_REG_DATA0, (*(buf+3) << 24));
		case 7:
			ra_or(SPI_REG_DATA0, (*(buf+2) << 16));
		case 6:
			ra_or(SPI_REG_DATA0, (*(buf+1) << 8));
		case 5:
		case 2:
			ra_or(SPI_REG_DATA0, *buf);
			break;
		default:
			printk("%s: fixme, write of length %d\n", __func__, n_tx);
			return -1;
		}
	}

	/* step 3. set mosi_byte_cnt */
	ra_and(SPI_REG_CTL, ~SPI_CTL_TX_RX_CNT_MASK);
	ra_or(SPI_REG_CTL, (n_rx << 4));
#ifdef MX_4B_MODE 
	if (flash && flash->chip->addr4b && n_tx >= 4)
		ra_or(SPI_REG_CTL, (n_tx + 1));
	else
#endif
		ra_or(SPI_REG_CTL, n_tx);

	/* step 4. kick */
	ra_or(SPI_REG_CTL, SPI_CTL_START);

	/* step 5. wait spi_master_busy */
	bbu_spic_busy_wait();
	if (flag & SPIC_WRITE_BYTES)
		return 0;

	/* step 6. read DI/DO data #0 */
	if (flag & SPIC_READ_BYTES) {
		if (buf == NULL) {
			printk("%s: read null buf\n", __func__);
			return -1;
		}
		reg = ra_inl(SPI_REG_DATA0);
		switch (n_rx) {
		case 4:
			*(buf+3) = (u8)(reg >> 24);
		case 3:
			*(buf+2) = (u8)(reg >> 16);
		case 2:
			*(buf+1) = (u8)(reg >> 8);
		case 1:
			*buf = (u8)reg;
			break;
		default:
			printk("%s: fixme, read of length %d\n", __func__, n_rx);
			return -1;
		}
	}
	return 0;
}
#endif // BBU_MODE //

/*
 * read SPI flash device ID
 */
static int raspi_read_devid(u8 *rxbuf, int n_rx)
{
	u8 code = OPCODE_RDID;
	int retval;

#ifdef USER_MODE
	retval = spic_read(&code, 1, rxbuf, n_rx);
#elif defined BBU_MODE
	retval = bbu_spic_trans(code, 0, rxbuf, 1, 3, SPIC_READ_BYTES);
	if (!retval)
		retval = n_rx;
#endif
	if (retval != n_rx) {
		printk("%s: ret: %x\n", __func__, retval);
		return retval;
	}
	return retval;
}

/*
 * Read the status register, returning its value in the location
 */
static int raspi_read_sr(u8 *val)
{
	ssize_t retval;
	u8 code = OPCODE_RDSR;

#ifdef USER_MODE
	retval = spic_read(&code, 1, val, 1);
#elif defined BBU_MODE
	retval = bbu_spic_trans(code, 0, val, 1, 1, SPIC_READ_BYTES);
	return retval;
#endif
	if (retval != 1) {
		printk("%s: ret: %x\n", __func__, retval);
		return -EIO;
	}
	return 0;
}

/*
 * write status register
 */
static int raspi_write_sr(u8 *val)
{
	ssize_t retval;
	u8 code = OPCODE_WRSR;

#ifdef USER_MODE
	retval = spic_write(&code, 1, val, 1);
#elif defined BBU_MODE
	retval = bbu_spic_trans(code, 0, val, 2, 0, SPIC_WRITE_BYTES);
#endif
	if (retval != 1) {
		printk("%s: ret: %x\n", __func__, retval);
		return -EIO;
	}
	return 0;
}

#ifdef MX_4B_MODE
static int raspi_4byte_mode(int enable)
{
	ssize_t retval;
	u8 code;

	code = enable? 0xB7 : 0xE9; /* B7: enter 4B, E9: exit 4B */

#ifdef USER_MODE
	if (enable)
		ra_or(RT2880_SPICFG_REG, SPICFG_ADDRMODE);
	else
		ra_and(RT2880_SPICFG_REG, ~(SPICFG_ADDRMODE));
	retval = spic_read(&code, 1, 0, 0);
#elif defined BBU_MODE
	if (enable) {
		ra_or(SPI_REG_CTL, 0x3 << 19);
		ra_or(SPI_REG_Q_CTL, 0x3 << 8);
	}
	else {
		ra_and(SPI_REG_CTL, ~SPI_CTL_SIZE_MASK);
		ra_or(SPI_REG_CTL, 0x2 << 19);
		ra_and(SPI_REG_Q_CTL, ~(0x3 << 8));
		ra_or(SPI_REG_Q_CTL, 0x2 << 8);
	}
	retval = bbu_spic_trans(code, 0, NULL, 1, 0, 0);
#endif
	if (retval != 0) {
		printk("%s: ret: %x\n", __func__, retval);
		return -1;
	}
	return 0;
}
#endif // MX_4B_MODE //

/*
 * Set write enable latch with Write Enable command.
 * Returns negative if error occurred.
 */
static inline int raspi_write_enable(void)
{
	u8 code = OPCODE_WREN;

#ifdef USER_MODE
	return spic_write(&code, 1, NULL, 0);
#elif defined BBU_MODE
	return bbu_spic_trans(code, 0, NULL, 1, 0, 0);
#endif
}

/*
 * Set all sectors (global) unprotected if they are protected.
 * Returns negative if error occurred.
 */
static inline int raspi_unprotect(void)
{
	u8 sr = 0;

	if (raspi_read_sr(&sr) < 0) {
		printk("%s: read_sr fail: %x\n", __func__, sr);
		return -1;
	}

	if ((sr & (SR_BP0 | SR_BP1 | SR_BP2)) != 0) {
		sr = 0;
		raspi_write_sr(&sr);
	}

	return 0;
}

/*
 * Service routine to read status register until ready, or timeout occurs.
 * Returns non-zero if error.
 */
static int raspi_wait_ready(int sleep_ms)
{
	int count;
	int sr = 0;

	/*int timeout = sleep_ms * HZ / 1000;
	while (timeout) 
		timeout = schedule_timeout (timeout);*/

	/* one chip guarantees max 5 msec wait here after page writes,
	 * but potentially three seconds (!) after page erase.
	 */
	for (count = 0; count < ((sleep_ms+1) *1000 * 500); count++) {
		if ((raspi_read_sr((u8 *)&sr)) < 0)
			break;
		else if (!(sr & (SR_WIP | SR_EPE))) {
			return 0;
		}

		udelay(1);
		/* REVISIT sometimes sleeping would be best */
	}

	printk("%s: read_sr fail: %x\n", __func__, sr);
	return -EIO;
}

static int raspi_wait_sleep_ready(int sleep_ms)
{
	int count;
	int sr = 0;

	/*int timeout = sleep_ms * HZ / 1000;
	while (timeout) 
		timeout = schedule_timeout (timeout);*/

	/* one chip guarantees max 5 msec wait here after page writes,
	 * but potentially three seconds (!) after page erase.
	 */
	for (count = 0; count < ((sleep_ms+1) *1000); count++) {
		if ((raspi_read_sr((u8 *)&sr)) < 0)
			break;
		else if (!(sr & (SR_WIP | SR_EPE))) {
			return 0;
		}

		usleep(1);
		/* REVISIT sometimes sleeping would be best */
	}

	printk("%s: read_sr fail: %x\n", __func__, sr);
	return -EIO;
}

/*
 * Erase one sector of flash memory at offset ``offset'' which is any
 * address within the sector which should be erased.
 *
 * Returns 0 if successful, non-zero otherwise.
 */
static int raspi_erase_sector(u32 offset)
{

	u8 opcode = OPCODE_SE;

	// Here's an ugly magic number.  For the partition at this address,
	//  I don't want to erase the entire partition.  I only want to 
	//  erase the first 4K block because there is important 
	//  manufacturing data at a later block.  I may play with erasesize
	//  at some time, but that requires app-layer changes also.
	if (offset == 0x30000) {
		opcode = OPCODE_P4E;
	}

	/* Wait until finished previous write command. */
	if (raspi_wait_ready(3))
		return -EIO;

	/* Send write enable, then erase commands. */
	raspi_write_enable();
	raspi_unprotect();

#ifdef USER_MODE
#ifdef MX_4B_MODE
	if (flash->chip->addr4b) {
		raspi_4byte_mode(1);
		flash->command[0] = opcode;
		flash->command[1] = offset >> 24;
		flash->command[2] = offset >> 16;
		flash->command[3] = offset >> 8;
		flash->command[4] = offset;
		spic_write(flash->command, 5, 0 , 0);
		raspi_wait_sleep_ready(950);
		raspi_4byte_mode(0);
		return 0;
	}
#endif // MX_4B_MODE //

	/* Set up command buffer. */
	flash->command[0] = opcode;
	flash->command[1] = offset >> 16;
	flash->command[2] = offset >> 8;
	flash->command[3] = offset;

	spic_write(flash->command, 4, 0 , 0);
	raspi_wait_sleep_ready(950);
#elif defined BBU_MODE
#ifdef MX_4B_MODE 
	if (flash->chip->addr4b)
		raspi_4byte_mode(1);
#endif
	bbu_spic_trans(STM_OP_SECTOR_ERASE, offset, NULL, 4, 0, 0);
	//raspi_wait_ready(950);
	raspi_wait_sleep_ready(950);
#ifdef MX_4B_MODE 
	if (flash->chip->addr4b)
		raspi_4byte_mode(0);
#endif
#endif
	return 0;
}

/*
 * SPI device driver setup and teardown
 */
struct chip_info *chip_prob(void)
{
	struct chip_info *info, *match;
	u8 buf[5];
	u32 jedec, weight;
	int i;

	raspi_read_devid(buf, 5);
	jedec = (u32)((u32)(buf[1] << 24) | ((u32)buf[2] << 16) | ((u32)buf[3] <<8) | (u32)buf[4]);

#ifdef BBU_MODE
	printk("flash manufacture id: %x, device id %x %x\n", buf[0], buf[1], buf[2]);
#else
	printk("device id : %x %x %x %x %x (%x)\n", buf[0], buf[1], buf[2], buf[3], buf[4], jedec);
#endif
	
	// FIXME, assign default as AT25D
	weight = 0xffffffff;
	match = &chips_data[0];
	for (i = 0; i < ARRAY_SIZE(chips_data); i++) {
		info = &chips_data[i];
		if (info->id == buf[0]) {
#ifdef BBU_MODE
			if ((u8)(info->jedec_id >> 24 & 0xff) == buf[1] &&
			    (u8)(info->jedec_id >> 16 & 0xff) == buf[2])
#else
			if (info->jedec_id == jedec)
#endif
				return info;

			if (weight > (info->jedec_id ^ jedec)) {
				weight = info->jedec_id ^ jedec;
				match = info;
			}
		}
	}
	printk("Warning: un-recognized chip ID, please update SPI driver!\n");

	return match;
}

int raspi_set_lock (struct mtd_info *mtd, loff_t to, size_t len, int set)
{
	u32 page_offset, page_size;
	int retval;

	/*  */
	while (len > 0) {
		/* FIXME: 4b mode ? */
		/* write the next page to flash */
		flash->command[0] = (set == 0)? 0x39 : 0x36;
		flash->command[1] = to >> 16;
		flash->command[2] = to >> 8;
		flash->command[3] = to;

		raspi_wait_ready(1);
			
		raspi_write_enable();

#ifdef USER_MODE
		retval = spic_write(flash->command, 4, 0, 0);
#elif defined BBU_MODE
		retval = 0;
		//FIXME
#endif
			
		if (retval < 0) {
			return -EIO;
		}
			
		page_offset = (to & (mtd->erasesize-1));
		page_size = mtd->erasesize - page_offset;
				
		len -= mtd->erasesize;
		to += mtd->erasesize;
	}

	return 0;
	
}


/*
 * MTD implementation
 */

/*
 * Erase an address range on the flash chip.  The address range may extend
 * one or more erase sectors.  Return an error is there is a problem erasing.
 */
static int ramtd_erase(struct mtd_info *mtd, struct erase_info *instr)
{
	u32 addr,len;

	//printk("%s: addr:%x len:%x\n", __func__, instr->addr, instr->len);
	/* sanity checks */
	if (instr->addr + instr->len > flash->mtd.size)
		return -EINVAL;

#if 0  //FIXME - undefined reference to `__umoddi3'
	if ((instr->addr % mtd->erasesize) != 0
			|| (instr->len % mtd->erasesize) != 0) {
		return -EINVAL;
	}
#endif

	addr = instr->addr;
	len = instr->len;

  	down(&flash->lock);

	/* now erase those sectors */
	while (len > 0) {
		if (raspi_erase_sector(addr)) {
			instr->state = MTD_ERASE_FAILED;
			up(&flash->lock);
			return -EIO;
		}

		addr += mtd->erasesize;
		len -= mtd->erasesize;
	}

  	up(&flash->lock);

	instr->state = MTD_ERASE_DONE;
	mtd_erase_callback(instr);

	return 0;
}

/*
 * Read an address range from the flash chip.  The address range
 * may be any size provided it is within the physical boundaries.
 */
static int ramtd_read(struct mtd_info *mtd, loff_t from, size_t len,
	size_t *retlen, u_char *buf)
{
	size_t rdlen = 0;

	ra_dbg("%s: from:%x len:%x \n", __func__, (unsigned int)from, len);

	/* sanity checks */
	if ((len == 0) || (flash == NULL))
		return 0;

	if (from + len > flash->mtd.size)
		return -EINVAL;

	/* Byte count starts at zero. */
	if (retlen)
		*retlen = 0;

	down(&flash->lock);

	/* Wait till previous write/erase is done. */
	if (raspi_wait_ready(1)) {
		/* REVISIT status return?? */
		up(&flash->lock);
		return -EIO;
	}

#ifdef USER_MODE

	/* Set up the write data buffer. */
#ifdef MX_4B_MODE
	if (flash->chip->addr4b) {
		raspi_4byte_mode(1);
		flash->command[0] = OPCODE_READ;
		flash->command[1] = from >> 24;
		flash->command[2] = from >> 16;
		flash->command[3] = from >> 8;
		flash->command[4] = from;
		rdlen = spic_read(flash->command, 5, buf, len);
		raspi_4byte_mode(0);
	}
	else
#endif
	{
		flash->command[0] = OPCODE_FAST_READ;
		flash->command[1] = from >> 16;
		flash->command[2] = from >> 8;
		flash->command[3] = from;
		flash->command[4] = 0;
		rdlen = spic_read(flash->command, 5, buf, len);
	}
#elif defined BBU_MODE
#ifdef MX_4B_MODE 
	if (flash->chip->addr4b)
		raspi_4byte_mode(1);
#endif
	do {
		int rc, more;
#ifdef MORE_BUF_MODE
		more = 32;
#else
		more = 4;
#endif
		if (len - rdlen <= more) {
#ifdef MORE_BUF_MODE
			rc = bbu_mb_spic_trans(STM_OP_RD_DATA, from, (buf+rdlen), 0, (len-rdlen), SPIC_READ_BYTES);
#else
			rc = bbu_spic_trans(STM_OP_RD_DATA, from, (buf+rdlen), 4, (len-rdlen), SPIC_READ_BYTES);
#endif
			if (rc != 0) {
				printk("%s: failed\n", __func__);
				break;
			}
			rdlen = len;
		}
		else {
#ifdef MORE_BUF_MODE
			rc = bbu_mb_spic_trans(STM_OP_RD_DATA, from, (buf+rdlen), 0, more, SPIC_READ_BYTES);
#else
			rc = bbu_spic_trans(STM_OP_RD_DATA, from, (buf+rdlen), 4, more, SPIC_READ_BYTES);
#endif
			if (rc != 0) {
				printk("%s: failed\n", __func__);
				break;
			}
			rdlen += more;
			from += more;
		}
	} while (rdlen < len);
#endif

  	up(&flash->lock);

	if (retlen) 
		*retlen = rdlen;

	if (rdlen != len) 
		return -EIO;

	return 0;
}

inline int ramtd_lock (struct mtd_info *mtd, loff_t to, uint64_t len)
{
	//return raspi_set_lock(mtd, to, len, 1);
	return 0;
}

inline int ramtd_unlock (struct mtd_info *mtd, loff_t to, uint64_t len)
{
	//return raspi_set_lock(mtd, to, len, 0);
	return 0;
}


/*
 * Write an address range to the flash chip.  Data must be written in
 * FLASH_PAGESIZE chunks.  The address range may be any size provided
 * it is within the physical boundaries.
 */
static int ramtd_write(struct mtd_info *mtd, loff_t to, size_t len,
	size_t *retlen, const u_char *buf)
{
	u32 page_offset, page_size;
	int rc = 0;
#if defined BBU_MODE
	int wrto, wrlen, more;
	char *wrbuf;
#endif
	int count = 0;

	ra_dbg("%s: to:%llx len:%x \n", __func__, to, len);
	if (retlen)
		*retlen = 0;

	/* sanity checks */
	if (len == 0)
		return 0;
	if (to + len > flash->mtd.size)
		return -EINVAL;

  	down(&flash->lock);

	/* Wait until finished previous write command. */
	if (raspi_wait_ready(2)) {
		up(&flash->lock);
		return -1;
	}

#ifdef USER_MODE
	/* Set up the opcode in the write buffer. */
	flash->command[0] = OPCODE_PP;
#ifdef MX_4B_MODE
	if (flash->chip->addr4b) {
		flash->command[1] = to >> 24;
		flash->command[2] = to >> 16;
		flash->command[3] = to >> 8;
		flash->command[4] = to;
	}
	else
#endif
	{
		flash->command[1] = to >> 16;
		flash->command[2] = to >> 8;
		flash->command[3] = to;
	}
#endif

	/* what page do we start with? */
	page_offset = to % FLASH_PAGESIZE;

#ifdef MX_4B_MODE
	if (flash->chip->addr4b)
		raspi_4byte_mode(1);
#endif

	/* write everything in PAGESIZE chunks */
	while (len > 0) {
		page_size = min_t(size_t, len, FLASH_PAGESIZE-page_offset);
		page_offset = 0;

		/* write the next page to flash */
#ifdef USER_MODE
#ifdef MX_4B_MODE
		if (flash->chip->addr4b) {
			flash->command[1] = to >> 24;
			flash->command[2] = to >> 16;
			flash->command[3] = to >> 8;
			flash->command[4] = to;
		}
		else
#endif
		{
			flash->command[1] = to >> 16;
			flash->command[2] = to >> 8;
			flash->command[3] = to;
		}
#endif

		raspi_wait_ready(3);
		raspi_write_enable();
		raspi_unprotect();

#ifdef USER_MODE

#ifdef MX_4B_MODE
		if (flash->chip->addr4b)
			rc = spic_write(flash->command, 5, buf, page_size);
		else
#endif
			rc = spic_write(flash->command, 4, buf, page_size);

#elif defined BBU_MODE
		wrto = to;
		wrlen = page_size;
		wrbuf = buf;
		rc = wrlen;
		do {
#ifdef MORE_BUF_MODE
			more = 32;
#else
			more = 4;
#endif
			if (wrlen <= more) {
#ifdef MORE_BUF_MODE
				bbu_mb_spic_trans(STM_OP_PAGE_PGRM, wrto, wrbuf, wrlen, 0, SPIC_WRITE_BYTES);
#else
				bbu_spic_trans(STM_OP_PAGE_PGRM, wrto, wrbuf, wrlen+4, 0, SPIC_WRITE_BYTES);
#endif
				wrlen = 0;
			}
			else {
#ifdef MORE_BUF_MODE
				bbu_mb_spic_trans(STM_OP_PAGE_PGRM, wrto, wrbuf, more, 0, SPIC_WRITE_BYTES);
#else
				bbu_spic_trans(STM_OP_PAGE_PGRM, wrto, wrbuf, more+4, 0, SPIC_WRITE_BYTES);
#endif
				wrto += more;
				wrlen -= more;
				wrbuf += more;
			}
			if (wrlen > 0) {
				raspi_wait_ready(3);
				raspi_write_enable();
			}
		} while (wrlen > 0);
#endif
		//printk("%s : to:%llx page_size:%x ret:%x\n", __func__, to, page_size, rc);

		if (rc > 0) {
			if (retlen)
				*retlen += rc;
			if (rc < page_size) {
#ifdef MX_4B_MODE
				if (flash->chip->addr4b)
					raspi_4byte_mode(0);
#endif
				up(&flash->lock);
				printk("%s: rc:%x return:%x page_size:%x \n", 
				       __func__, rc, rc, page_size);
				return -EIO;
			}
		}

		len -= page_size;
		to += page_size;
		buf += page_size;
		count++;
		if ((count & 0xf) == 0)
			raspi_wait_sleep_ready(1);
			
	}

#ifdef MX_4B_MODE
	if (flash->chip->addr4b)
		raspi_4byte_mode(0);
#endif

	up(&flash->lock);

	return 0;
}

/*
 * board specific setup should have ensured the SPI clock used here
 * matches what the READ command supports, at least until this driver
 * understands FAST_READ (for clocks over 25 MHz).
 */
static struct mtd_info *raspi_probe(struct map_info *map)
{
	unsigned i;
	struct chip_info *chip;

	if (flash == NULL) {
		return NULL;
	}

	chip = flash->chip;

	//printk("%s\n", __FUNCTION__);

	printk("%s(%02x %04x) (%lld Kbytes)\n", 
	       chip->name, chip->id, chip->jedec_id, flash->mtd.size / 1024);

	printk("mtd .name = %s, .size = 0x%.8llx (%lluM) "
			".erasesize = 0x%.8x (%uK) .numeraseregions = %d\n",
 		flash->mtd.name,
		flash->mtd.size, flash->mtd.size / (1024*1024),
		flash->mtd.erasesize, flash->mtd.erasesize / 1024,
 		flash->mtd.numeraseregions);

	if (flash->mtd.numeraseregions)
		for (i = 0; i < flash->mtd.numeraseregions; i++)
			printk("mtd.eraseregions[%d] = { .offset = 0x%.8llx, "
				".erasesize = 0x%.8x (%uK), "
				".numblocks = %d }\n",
				i, flash->mtd.eraseregions[i].offset,
				flash->mtd.eraseregions[i].erasesize,
				flash->mtd.eraseregions[i].erasesize / 1024,
				flash->mtd.eraseregions[i].numblocks);

	mtd_device_register(&flash->mtd, rt2880_partitions, nr_parts);

	return &flash->mtd;
}


static void raspi_destroy(struct mtd_info *mtd)
{
	int			status;

	/* Clean up MTD stuff. */
	status = mtd_device_unregister(&flash->mtd);
	if (status == 0) {
		kfree(flash);
	}
}

static struct mtd_chip_driver raspi_chipdrv = {
        .probe   = raspi_probe,
        .destroy = raspi_destroy,
        .name    = "raspi_probe",
        .module  = THIS_MODULE
};

/*
 *	Set the Intel flash back to read mode since some old boot
 *	loaders don't.
 */
static int raspi_reboot_notifier(struct notifier_block *nb, unsigned long val, void *v)
{

#ifdef MX_4B_MODE
	int retval;
	u8 code, scur;
	int timeout = 10;
	struct mtd_info mtd;
	u_char buf[256];
	int len;

	raspi_4byte_mode(0);	

	/*  
		This is an odd-looking hack.  On some hardware (early rev
		Pearl for example) if the last flash activity was an 
		erase/write cycle, the router will not reboot correctly 
		without a power cycle.  This read makes the system happier.
	*/
	mtd.size = 0x10000;
	ramtd_read(&mtd, 0x30000, 0x100, &len, &buf[0]);

	scur = SECR_4BYTE;
	while ((scur & SECR_4BYTE) && timeout) {
		timeout--;
		code = READ_SCUR;
		retval = spic_transfer(&code, 1, &scur, 1, SPIC_READ_BYTES);
		if (retval != 0) {
			printk("read scur failed\n");
			scur = SECR_4BYTE;
		} else {
			//printf("scur 0x%x\n", scur);
			if (scur & SECR_4BYTE) {
				//printf("Still in 4 byte mode\n");
				code = 0xE9; /* EX4B, exit 4-byte mode */
				retval = spic_transfer(&code, 1, NULL, 0, SPIC_READ_BYTES);
				if (retval != 0) {
					printk("%s: ret: %x\n", __func__, retval);
					return(NOTIFY_OK);
				}
				usleep(500);
			}
		}
	}

#endif

	return(NOTIFY_OK);
}


static struct notifier_block raspi_notifier_block = {
	raspi_reboot_notifier, NULL, 0
};

static int __init raspi_init(void)
{
#if defined (CONFIG_RT6855A_FPGA)
	// only for FPGA: restore to registers since PCI initiation changes them.
	ra_outl(0xbfbc0028, 0x68880);
	ra_outl(0xbfbc0004, 0xe9);
	ra_outl(0xbfbc0008, 0xffffffff);
	ra_outl(0xbfbc0000, 0x160001);
#endif

	register_mtd_chip_driver(&raspi_chipdrv);
	
	raspi_chipdrv.probe(NULL);
	
	register_reboot_notifier(&raspi_notifier_block);

	return 0;
}

static void __exit raspi_exit(void)
{
    unregister_mtd_chip_driver(&raspi_chipdrv);
	unregister_reboot_notifier(&raspi_notifier_block);
}


module_init(raspi_init);
module_exit(raspi_exit);

/*
  Create dynamic MTD mappings.  There are hints in
  the flash image for kernel and filesystem sizes.
  Use those for the mapping, and the Upgrade
  partition gets all free flash.
*/
void __init adjust_dynamic_maps(struct mtd_partition *parts,
								int number_partitions)
{
	int i;
	u32 size;
	u32 aligned_size = 0;
	u32 magic;
	size_t retlen;
	u32 address;

	int kernel = 0, filesystem = 0, upgrade = 0;

	// Fake needed for the MTD Read function calls
	struct mtd_info mtd;

#define KERNEL_MAGIC 0x27051956
#define FS_MAGIC 0x27051958

	//printk("%s\n", __FUNCTION__);

#if 0
	printk("parts 0x%p, number_partitions %d\n", parts, number_partitions);
	if (0 == parts) return;
	for (i = 0; i < number_partitions; i++) {
		printk("name %s, size 0x%llx, offset 0x%llx, flags 0x%x \n", 
			   parts[i].name, parts[i].size, parts[i].offset, parts[i].mask_flags); 
	}
#endif

	/*  Let's determine partitions based on names */
	for (i = 0; i < number_partitions; i++) {
		if (strncmp(parts[i].name, "Kernel", strlen("Kernel")) == 0) {
			kernel = i;
		}
		if (strncmp(parts[i].name, "RootFS", strlen("RootFS")) == 0) {
			filesystem = i;
		}
		if (strncmp(parts[i].name, "Upgrade", strlen("Upgrade")) == 0) {
			upgrade = i;
		}
	}

	// SPI flash needs a read function to get a value

	// Find/set kernel size
	address = MTD_BOOT_PART_SIZE + MTD_CONFIG_PART_SIZE + MTD_FACTORY_PART_SIZE;
	ramtd_read(&mtd, address, sizeof(u32), &retlen, (char *)&magic); 
	//printk("0x%x, %x\n", address, ntohl(magic));

	if ((ntohl(magic) == KERNEL_MAGIC) && kernel) {
		ramtd_read(&mtd, address + 12, sizeof(u32), &retlen, (char *)&size); 
		size = ntohl(size);
		//printk("%x\n", size);
		aligned_size = (size + 0x20000) & ~(0x20000 - 1);
		//printk("%x\n", aligned_size);
		parts[kernel].size = aligned_size;
	}

	// Find/Set Filesystem size
	address = address + aligned_size - 16;
	ramtd_read(&mtd, address, sizeof(u32), &retlen, (char *)&magic); 
	//printk("0x%x %x\n", address, ntohl(magic));

	if ((ntohl(magic) == FS_MAGIC) && filesystem) {
		ramtd_read(&mtd, address + 8, sizeof(u32), &retlen, (char *)&size); 
		//printk("%x\n", size);
		//size = ntohl(size);  // size is aligned correctly
		aligned_size = (size + 0x20000) & ~(0x20000 - 1);
		parts[filesystem].size = aligned_size;
	}

	// Upgrade partition has all the rest of the flash
#ifdef CONFIG_MTD_TESTS_MODULE
	//  If I'm using MTD tests, then I want a smaller
	// partition to play in.  
	if (upgrade && (parts[upgrade].size == 0))
#endif
	{
		if (upgrade) {
			parts[upgrade].size = MTD_PART_SIZE;
			for (i = 0; i < number_partitions; i++) {
				if (i != upgrade) 
					parts[upgrade].size -= parts[i].size;
			}
		}
	}

#if 0
	if (0 == parts) return;
	for (i = 0; i < number_partitions; i++) {
		printk("name %s, size 0x%x, offset 0x%x, flags 0x%x \n", 
			   parts[i].name, parts[i].size, parts[i].offset, parts[i].mask_flags); 
	}
#endif

}

static int physmap_flash_probe(struct platform_device *dev)
{
	struct physmap_flash_data *physmap_data;
	struct chip_info		*chip;

	//printk("%s init\n", __FUNCTION__);

	physmap_data = dev->dev.platform_data;

	if (physmap_data == NULL)
		return -ENODEV;
	
	// We do some chip driver setup here, rather than in
	//  raspi_probe.  We need to be able to read some
	//  data from the flash part.

	spic_init();
	
	chip = chip_prob();
	
	flash = kzalloc(sizeof *flash, GFP_KERNEL);
	if (!flash)
		return -ENODEV;

	sema_init(&flash->lock, 1);

	flash->chip = chip;
	flash->mtd.name = "raspi";

	flash->mtd.type = MTD_NORFLASH;
	flash->mtd.writesize = 1;
	flash->mtd.flags = MTD_CAP_NORFLASH;
	flash->mtd.size = chip->sector_size * chip->n_sectors;
	flash->mtd.erasesize = chip->sector_size;
	flash->mtd._erase = ramtd_erase;
	flash->mtd._read = ramtd_read;
	flash->mtd._write = ramtd_write;
	flash->mtd._lock = ramtd_lock;
	flash->mtd._unlock = ramtd_unlock;

	rt2880_partitions = physmap_data->parts;
	nr_parts = physmap_data->nr_parts;
	adjust_dynamic_maps(physmap_data->parts, physmap_data->nr_parts);

	return 0;
}

static const struct of_device_id ralink_mtd_match[] = {
  { .compatible = "ralink,nor" },
  {},
};
MODULE_DEVICE_TABLE(of, ralink_mtd_match);

static struct platform_driver physmap_flash_driver = {
	.probe		= physmap_flash_probe,
	.driver		= {
		.name	= "ralink-flash",
		.owner	= THIS_MODULE,
		.of_match_table = ralink_mtd_match,
	},
};

static int __init ralink_physmap_init(void)
{
	int err;

	//printk("*SPI* %s\n", __FUNCTION__);

	err = platform_driver_register(&physmap_flash_driver);
	return err;
}

static void __exit ralink_physmap_exit(void)
{
	platform_driver_unregister(&physmap_flash_driver);
}

module_init(ralink_physmap_init);
module_exit(ralink_physmap_exit);

MODULE_LICENSE("GPL");
MODULE_AUTHOR("Steven Liu");
MODULE_DESCRIPTION("MTD SPI driver for Ralink flash chips");

int ra_mtd_write_nm(char *name, loff_t to, size_t len, const u_char *buf)
{
	int ret = -1;
	size_t rdlen, wrlen;
	struct mtd_info *mtd;
	struct erase_info ei;
	u_char *bak = NULL;

	mtd = get_mtd_device_nm(name);
	if (IS_ERR(mtd))
		return (int)mtd;
	if (len > mtd->erasesize) {
		put_mtd_device(mtd);
		return -E2BIG;
	}

	bak = kmalloc(mtd->erasesize, GFP_KERNEL);
	if (bak == NULL) {
		put_mtd_device(mtd);
		return -ENOMEM;
	}

	ret = mtd_read(mtd, 0, mtd->erasesize, &rdlen, bak);
	if (ret != 0) {
		put_mtd_device(mtd);
		kfree(bak);
		return ret;
	}
	if (rdlen != mtd->erasesize)
		printk("warning: ra_mtd_write: rdlen is not equal to erasesize\n");

	memcpy(bak + to, buf, len);

	ei.mtd = mtd;
	ei.callback = NULL;
	ei.addr = 0;
	ei.len = mtd->erasesize;
	ei.priv = 0;
	ret = mtd_erase(mtd, &ei);
	if (ret != 0) {
		put_mtd_device(mtd);
		kfree(bak);
		return ret;
	}

	ret = mtd_write(mtd, 0, mtd->erasesize, &wrlen, bak);

	put_mtd_device(mtd);
	kfree(bak);
	return ret;
}

int ra_mtd_read_nm(char *name, loff_t from, size_t len, u_char *buf)
{
	int ret;
	size_t rdlen;
	struct mtd_info *mtd;

	mtd = get_mtd_device_nm(name);
	if (IS_ERR(mtd))
		return (int)mtd;

	ret = mtd_read(mtd, from, len, &rdlen, buf);
	if (rdlen != len)
		printk("warning: ra_mtd_read_nm: rdlen is not equal to len\n");

	put_mtd_device(mtd);
	return ret;
}

EXPORT_SYMBOL(ra_mtd_write_nm);
EXPORT_SYMBOL(ra_mtd_read_nm);