#include <ultra64.h>
#include "bcp.h"
#include "bbnand.h"
#include "bbfs.h"
#include "os_bbcard.h"
#include "bbint.h"
#include "piint.h"

#define VERBOSE(x)
#ifdef _DEBUG
u8 __osBbCardNoEcc;
#endif

#define POLL
#ifdef POLL
#define __INTR	0
#else
#define __INTR	PI_FLASH_CTRL_INTR
#endif

static void
fill_page(unsigned dev, unsigned addr, int which_buf, int wait) {
#ifdef _DEBUG
    IO_WRITE(PI_FLASH_ADDR_REG, addr << PI_FLASH_PAGE_ADDR_SHIFT);
    IO_WRITE(PI_FLASH_CTRL_REG, PI_FLASH_CTRL_START |
	    			__INTR		    |
	    			PI_FLASH_CTRL_WDPH  |
				0xf << PI_FLASH_CTRL_ADPH_SHIFT |
				NAND_CMD_DATA_INPUT << PI_FLASH_CTRL_CMD_SHIFT |
				which_buf << PI_FLASH_CTRL_BUF_SHIFT |
				dev << PI_FLASH_CTRL_DEV_SHIFT |
				((addr & 0x1f)==0x1f && __osBbCardNoEcc ? 0 : PI_FLASH_CTRL_ECC)  |
				PI_FLASH_CTRL_MCMD |
				528);
    if (__osBbCardNoEcc && (addr & 0x1f)==0x1f) osSyncPrintf("write no ecc\n");
#else
    IO_WRITE(PI_FLASH_ADDR_REG, addr << PI_FLASH_PAGE_ADDR_SHIFT);
    IO_WRITE(PI_FLASH_CTRL_REG, PI_FLASH_CTRL_START |
	    			__INTR              |
	    			PI_FLASH_CTRL_WDPH  |
				0xf << PI_FLASH_CTRL_ADPH_SHIFT |
				NAND_CMD_DATA_INPUT << PI_FLASH_CTRL_CMD_SHIFT |
				which_buf << PI_FLASH_CTRL_BUF_SHIFT |
				dev << PI_FLASH_CTRL_DEV_SHIFT |
				PI_FLASH_CTRL_ECC |
				PI_FLASH_CTRL_MCMD |
				528);
#endif
#ifdef POLL
    if (wait) do {
	if (IO_READ(MI_EINTR_REG)&MI_EINTR_MODULE_REMOVED){ 
            return /*1*/;
        }
    } while(IO_READ(PI_FLASH_CTRL_REG)&PI_FLASH_CTRL_BUSY);
#endif
}

static void
write_page(unsigned dev) {
    IO_WRITE(PI_FLASH_CTRL_REG, PI_FLASH_CTRL_START |
	    			PI_FLASH_CTRL_INTR  |
				NAND_CMD_PROGRAM << PI_FLASH_CTRL_CMD_SHIFT |
				PI_FLASH_CTRL_WRDY |
				dev << PI_FLASH_CTRL_DEV_SHIFT);
    do {
	if (IO_READ(MI_EINTR_REG)&MI_EINTR_MODULE_REMOVED){
            return /*1*/;
        }
    } while(IO_READ(PI_FLASH_CTRL_REG)&PI_FLASH_CTRL_BUSY);
}

static void
write_dummy(u32 dev) {
    IO_WRITE(PI_FLASH_CTRL_REG, PI_FLASH_CTRL_START |
	    			__INTR		    |
				NAND_CMD_DUMMY_PROGRAM << PI_FLASH_CTRL_CMD_SHIFT |
				PI_FLASH_CTRL_WRDY |
				dev << PI_FLASH_CTRL_DEV_SHIFT);
#ifdef POLL
    do {
	if (IO_READ(MI_EINTR_REG)&MI_EINTR_MODULE_REMOVED){
            return /*1*/;
        }
    } while(IO_READ(PI_FLASH_CTRL_REG)&PI_FLASH_CTRL_BUSY);
#endif
}

s32
osBbCardWriteBlock(u32 dev, u16 block, const void* addr, const void* spare) {
    u32 i, b = 0;
    s32 rv;
    u8 tmp;
VERBOSE(osSyncPrintf("write block %d\n", block));
#ifdef _DEBUG
    if ((u32)addr&15) return BBCARD_ERR_INVALID;
#endif
    if ((rv = __osBbCardGetAccess()) < 0) return rv;
    if (spare) {
	const u8 *p = spare;
	for(i = 0; i < BB_FL_SPARE_SIZE; i+=4) {
	    u32 x = p[0] << 24 | p[1] << 16 | p[2] << 8 | p[3];
	    IO_WRITE(PI_BUFFER_0_OOB_START+i, x);
	    IO_WRITE(PI_BUFFER_1_OOB_START+i, x);
	    p += 4;
	}
    } else {
	for(i = 0; i < BB_FL_SPARE_SIZE; i+=4) {
	    IO_WRITE(PI_BUFFER_0_OOB_START+i, 0xffffffff);
	    IO_WRITE(PI_BUFFER_1_OOB_START+i, 0xffffffff);
	}

    }
    __osBbCardDmaCopy(b, (void*)addr, OS_WRITE);
    for(i = 0; i < BB_FL_BLOCK_PAGES; i++) {
	fill_page(dev, BB_FL_BYTE_TO_PAGE(BB_FL_BLOCK_TO_BYTE(block)+i*BB_FL_PAGE_SIZE), b, 1);
#ifndef POLL
	if ((rv = __osBbCardWaitEvent()) < 0) goto error;
#endif
	write_page(dev);
	/* overlap next transfer */
	if (i < BB_FL_BLOCK_PAGES-1)
	    __osBbCardDmaCopy(b^1, (void*)addr+BB_FL_PAGE_SIZE*(i+1), OS_WRITE);

	if ((rv = __osBbCardWaitEvent()) < 0 || __osBbCardStatus(dev, &tmp, b))
	    goto error;
	if (tmp != (NAND_STATUS_CMD_PASS|NAND_STATUS_WRITE_OK|NAND_STATUS_READY)) {
#ifdef _DEBUG
	    osSyncPrintf("block %d write failed 0x%x\n", block, tmp);
#endif
	    	rv = BBCARD_ERR_FAIL;
		goto error;
	}
	b ^= 1;
    }
error:
    __osBbCardRelAccess();
    return rv;
}

#define BLOCK_PLANE(baddr)  ((baddr)&3)
#define BLOCK_SEG(baddr)  ((baddr)>>12)
s32
osBbCardWriteBlocks(u32 dev, const u16 block[], u32 n, const void* addr, const void* spare) {
    u32 i, j, b = 0;
    s32 rv;
    u8 tmp;
VERBOSE(osSyncPrintf("write blocks %d %d\n", block[0], n));
#ifdef _DEBUG
    if ((u32)addr&15) return BBCARD_ERR_INVALID;
#endif
    if ((rv = __osBbCardGetAccess()) < 0) return rv;
    if (spare) {
	const u8 *p = spare;
	for(i = 0; i < BB_FL_SPARE_SIZE; i+=4) {
	    u32 x = p[0] << 24 | p[1] << 16 | p[2] << 8 | p[3];
	    IO_WRITE(PI_BUFFER_0_OOB_START+i, x);
	    IO_WRITE(PI_BUFFER_1_OOB_START+i, x);
	    p += 4;
	}
    } else {
	for(i = 0; i < BB_FL_SPARE_SIZE; i+=4) {
	    IO_WRITE(PI_BUFFER_0_OOB_START+i, 0xffffffff);
	    IO_WRITE(PI_BUFFER_1_OOB_START+i, 0xffffffff);
	}

    }
    for(j = 0; j < n;) {
	u32 plane = 1 << BLOCK_PLANE(block[j]);
	u32 seg = BLOCK_SEG(block[j]);
	int l, k = j+1;
	while(__osBbCardMultiplane && k < n) {
	    if (((1 << BLOCK_PLANE(block[k])) & plane) || seg != BLOCK_SEG(block[k])) break;
	    plane |= 1 << BLOCK_PLANE(block[k]);
	    k++;
	}
//osSyncPrintf("write block %d-%d\n", block[j],block[k-1]);
	for(i = 0; i < BB_FL_BLOCK_PAGES; i++) {
	    __osBbCardDmaCopy(b, (void*)addr+j*BB_FL_BLOCK_SIZE+BB_FL_PAGE_SIZE*i, OS_WRITE);
	    for(l = j; l < k; l++) {
		fill_page(dev, BB_FL_BYTE_TO_PAGE(BB_FL_BLOCK_TO_BYTE(block[l])+i*BB_FL_PAGE_SIZE), b, 0);
		if (l < k-1)
		    __osBbCardDmaCopy(b^1, (void*)addr+(l+1)*BB_FL_BLOCK_SIZE+BB_FL_PAGE_SIZE*i, OS_WRITE);
#ifndef POLL
		if ((rv = __osBbCardWaitEvent()) < 0) goto error;
#else
		do {
		    if (IO_READ(MI_EINTR_REG)&MI_EINTR_MODULE_REMOVED){
                        break;
                    }
		} while(IO_READ(PI_FLASH_CTRL_REG)&PI_FLASH_CTRL_BUSY);
#endif
		if (l==k-1) {
		    write_page(dev);
#ifdef POLL
		    if ((rv = __osBbCardWaitEvent()) < 0) goto error;
#endif
		} else {
		    write_dummy(dev);
		}
		b ^= 1;
	    }
#ifndef POLL
	    if ((rv = __osBbCardWaitEvent()) < 0) goto error;
#endif
	    if (__osBbCardStatus(dev, &tmp, 0)) {
		rv = BBCARD_ERR_CHANGED;
		goto error;
	    }
    	    if (tmp != (NAND_STATUS_CMD_PASS|NAND_STATUS_WRITE_OK|NAND_STATUS_READY)) {
		rv = BBCARD_ERR_FAIL;
	    	goto error;
	    }
	}
	j = k;
    }
error:
    __osBbCardRelAccess();
    return rv;
}