/*
 * drivers/ide/ppc/mpc5xxx_ide.c
 *
 * Driver for MPC5xxx on-chip IDE interface
 *
 *  Copyright (c) 2003 Mipsys - Benjamin Herrenschmidt
 *
 *  This program is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU General Public License
 *  as published by the Free Software Foundation; either version
 *  2 of the License, or (at your option) any later version.
 */

#include <linux/config.h>
#include <linux/types.h>
#include <linux/kernel.h>
#include <linux/sched.h>
#include <linux/init.h>
#include <linux/delay.h>
#include <linux/ide.h>
#include <linux/pci.h>
#include <asm/io.h>
#include <asm/irq.h>
#include <asm/mpc5xxx.h>

#include <bestcomm_api.h>

#ifdef CONFIG_UBOOT
#include <asm/ppcboot.h>
#endif

#include "mpc5xxx_ide.h"
#include "ide_modes.h"

#define MAX_DMA_BUFFERS  128

#define XLB_PRIO_ON	1
#define XLB_PRIO_CORE	1
#define XLB_PRIO_USB	2
#define XLB_PRIO_BC	1
#define XLB_PRIO_PCI	7

#undef DEBUG

#ifdef DEBUG
#define DPRINTK(fmt, args...)	printk(fmt, ## args)
#else
#define DPRINTK(fmt, args...)
#endif

/*
 * TODO: Move these to a structure in hwif datas in case
 * Motorola ever release a version with more than one IDE
 * channel
 */
static int			mpc5xxx_hwif_index;
static struct mpc5xxx_ata	*mpc5xxx_ataregs;
static struct mpc5xxx_xlb	*mpc5xxx_xlbregs;
static int			mpc5xxx_clock_period;
static struct mpc5xxx_ata_timings mpc5xxx_timings[2];
static struct mpc5xxx_xlb	*mpc5xxx_xlbregs;
#if MPC5xxx_IDE_HAS_DMA
static int			mpc5xxx_ata_sdma_nextbd;
static int 			mpc5xxx_ata_sdma_task;
static unsigned long		mpc5xxx_ata_dma_cur_addr;
static unsigned long		mpc5xxx_ata_dma_cur_len;
static int			mpc5xxx_ata_dma_cur_sg;
static int			mpc5xxx_ata_free_bds;
static int			mpc5xxx_ata_dma_last_write;
#endif

extern int ide_build_sglist (ide_hwif_t *hwif, struct request *rq, int ddir);
extern int ide_raw_build_sglist (ide_hwif_t *hwif, struct request *rq);


/*
 * Add count for hold state. (Check exactly what that means with spec)
 */
#define ATA_ADD_COUNT		1

/*
 * Timing calculation helpers
 */
#define CALC_CLK_VALUE_UP(v)	(((v) + mpc5xxx_clock_period - 1) / mpc5xxx_clock_period)
#define CALC_CLK_VALUE_DOWN(v)	((v) / mpc5xxx_clock_period)

/*
 * System clock cycle value (IPB)
 */
#define	CLK_IPB66MHZ	15
#define	CLK_IPB132MHZ	7

/* ATAPI-4 PIO specs */
/* numbers in ns, extrapolation done by code */
static int t0_spec[5]	=	{600,	383,	240,	180,	120};
static int t1_spec[5]	=	{ 70,	50,	30,	30,	25};
static int t2_8spec[5]	=	{290,	290,	290,	80,	70};
static int t2_16spec[5] =	{165,	125,	100,	80,	70};
static int t2i_spec[5]	=	{0,	0,	0,	70,	25};
static int t4_spec[5]	=	{30,	20,	15,	10,	10};
static int ta_spec[5]	=	{35,	35,	35,	35,	35};

#if MPC5xxx_IDE_HAS_DMA

/* ATAPI-4 MDMA specs */
/* numbers in ns, extrapolation done by code */
static int t0M_spec_66[3]   =   {32,  10,  8};
static int td_spec_66[3]    =   {15,  6,   5};
static int th_spec_66[3]    =   {2,   1,   1};
static int tj_spec_66[3]    =   {2,   1,   1};
static int tkw_spec_66[3]   =   {15,  4,   2};
static int tm_spec_66[3]    =   {4,   2,   2};
static int tn_spec_66[3]    =   {1,   1,   1};

static int t0M_spec_132[3]  =   {64,  20,  16};
static int td_spec_132[3]   =   {29,  11,  10};
static int th_spec_132[3]   =   {3,   2,   2};
static int tj_spec_132[3]   =   {3,   1,   1};
static int tkw_spec_132[3]  =   {29,  7,   4};
static int tm_spec_132[3]   =   {7,   4,   4};
static int tn_spec_132[3]   =   {2,   1,   1};

/* ATAPI-4 UDMA specs */
static int tcyc_spec_66[3]    = {7,   3,   3};
static int t2cyc_spec_66[3]   = {13,  6,   5};
static int tds_spec_66[3]     = {1,   1,   1};
static int tdh_spec_66[3]     = {1,   1,   1};
static int tdvs_spec_66[3]    = {4,   3,   2};
static int tdvh_spec_66[3]    = {1,   1,   1};
static int tfs_minspec_66[3]  = {15,  13,  11};
static int tli_maxspec_66[3]  = {9,   9,   9};
static int tmli_spec_66[3]    = {2,   2,   2};
static int taz_spec_66[3]     = {1,   1,   1};
static int tzah_spec_66[3]    = {2,   2,   2};
static int tenv_minspec_66[3] = {4,   4,   4};
static int tsr_spec_66[3]     = {3,   1,   1};
static int trfs_spec_66[3]    = {4,   3,   3};
static int trp_spec_66[3]     = {11,  9,   7};
static int tack_spec_66[3]    = {2,   2,   2};
static int tss_spec_66[3]     = {4,   4,   4};

static int tcyc_spec_132[3]   = {15,  10,  8};
static int t2cyc_spec_132[3]  = {30,  18,  16};
static int tds_spec_132[3]    = {2,   2,   1};
static int tdh_spec_132[3]    = {1,   1,   1};
static int tdvs_spec_132[3]   = {10,  7,   5};
static int tdvh_spec_132[3]   = {1,   1,   1};
static int tfs_minspec_132[3] = {30,  26,  22};
static int tli_maxspec_132[3] = {19,  19,  19};
static int tmli_spec_132[3]   = {3,   3,   3};
static int taz_spec_132[3]    = {3,   1,   1};
static int tzah_spec_132[3]   = {3,   3,   3};
static int tenv_minspec_132[3]= {9,   9,   9};
static int tsr_spec_132[3]    = {6,   3,   2};
static int trfs_spec_132[3]   = {9,   7,   6};
static int trp_spec_132[3]    = {22,  17,  14};
static int tack_spec_132[3]   = {3,   3,   3};
static int tss_spec_132[3]    = {7,   7,   7};

#endif /* MPC5xxx_IDE_HAS_DMA */

static u8 mpc5xxx_dma2pio (u8 xfer_rate)
{
	switch(xfer_rate) {
#if MPC5xxx_IDE_HAS_DMA
	case XFER_UDMA_6:
	case XFER_UDMA_5:
	case XFER_UDMA_4:
	case XFER_UDMA_3:
	case XFER_UDMA_2:
	case XFER_UDMA_1:
	case XFER_UDMA_0:
	case XFER_MW_DMA_2:
	case XFER_PIO_4:
		return 4;
	case XFER_MW_DMA_1:
	case XFER_PIO_3:
		return 3;
	case XFER_SW_DMA_2:
	case XFER_PIO_2:
		return 2;
	case XFER_MW_DMA_0:
	case XFER_SW_DMA_1:
	case XFER_SW_DMA_0:
#else
	case XFER_PIO_4:
		return 4;
	case XFER_PIO_3:
		return 3;
	case XFER_PIO_2:
		return 2;
#endif /* MPC5xxx_IDE_HAS_DMA */
	case XFER_PIO_1:
	case XFER_PIO_0:
	case XFER_PIO_SLOW:
	default:
		return 0;
	}
}

static void mpc5xxx_ide_apply_timings(ide_drive_t *drive)
{
	out_be32(&mpc5xxx_ataregs->pio1,  mpc5xxx_timings[drive->select.b.unit & 0x01].pio1);
	out_be32(&mpc5xxx_ataregs->pio2,  mpc5xxx_timings[drive->select.b.unit & 0x01].pio2);
	out_be32(&mpc5xxx_ataregs->mdma1, mpc5xxx_timings[drive->select.b.unit & 0x01].mdma1);
	out_be32(&mpc5xxx_ataregs->mdma2, mpc5xxx_timings[drive->select.b.unit & 0x01].mdma2);
	out_be32(&mpc5xxx_ataregs->udma1, mpc5xxx_timings[drive->select.b.unit & 0x01].udma1);
	out_be32(&mpc5xxx_ataregs->udma2, mpc5xxx_timings[drive->select.b.unit & 0x01].udma2);
	out_be32(&mpc5xxx_ataregs->udma3, mpc5xxx_timings[drive->select.b.unit & 0x01].udma3);
	out_be32(&mpc5xxx_ataregs->udma4, mpc5xxx_timings[drive->select.b.unit & 0x01].udma4);
	out_be32(&mpc5xxx_ataregs->udma5, mpc5xxx_timings[drive->select.b.unit & 0x01].udma5);
}

static void mpc5xxx_ide_tuneproc(ide_drive_t *drive, u8 pio)
{
	int which = drive->select.b.unit & 0x01;
	u32 t0, t2_8, t2_16, t2i, t4, t1, ta;

	pio = ide_get_best_pio_mode(drive, pio, 5, NULL);

	printk("%s: Setting PIO %d timings\n", drive->name, pio);

	t0 = ATA_ADD_COUNT + CALC_CLK_VALUE_UP(t0_spec[pio]);
	t2_8 = ATA_ADD_COUNT + CALC_CLK_VALUE_UP(t2_8spec[pio]);
	t2_16 = ATA_ADD_COUNT + CALC_CLK_VALUE_UP(t2_16spec[pio]);
	t2i = ATA_ADD_COUNT + CALC_CLK_VALUE_UP(t2i_spec[pio]);
	t4 = ATA_ADD_COUNT + CALC_CLK_VALUE_UP(t4_spec[pio]);
	t1 = ATA_ADD_COUNT + CALC_CLK_VALUE_UP(t1_spec[pio]);
	ta = ATA_ADD_COUNT + CALC_CLK_VALUE_UP(ta_spec[pio]);

	mpc5xxx_timings[which].pio1 = (t0 << 24) | (t2_8 << 16) | (t2_16 << 8) | (t2i);
	mpc5xxx_timings[which].pio2 = (t4 << 24) | (t1 << 16) | (ta << 8);

	if (drive->select.all == HWIF(drive)->INB(IDE_SELECT_REG))
		mpc5xxx_ide_apply_timings(drive);
}

#if MPC5xxx_IDE_HAS_DMA

static void mpc5xxx_ide_calc_udma_timings(ide_drive_t *drive, u8 speed)
{
	int which = drive->select.b.unit & 0x01;
	u32 t2cyc, tcyc, tds, tdh, tdvs, tdvh, tfs, tli, tmli, taz, tenv, tsr, tss, trfs, trp, tack, tzah;
    if (mpc5xxx_clock_period == CLK_IPB66MHZ) {
    
		t2cyc = (u32) t2cyc_spec_66[speed];
		tcyc = (u32) tcyc_spec_66[speed];
		tds = (u32) tds_spec_66[speed];
		tdh = (u32) tdh_spec_66[speed];
		tdvs = (u32) tdvs_spec_66[speed];
		tdvh = (u32) tdvh_spec_66[speed];
		tfs = (u32) tfs_minspec_66[speed];
		tmli = (u32) tmli_spec_66[speed];
		tenv = (u32) tenv_minspec_66[speed];
		tss = (u32) tss_spec_66[speed];
		trp = (u32) trp_spec_66[speed];
		tack = (u32) tack_spec_66[speed];
		tzah = (u32) tzah_spec_66[speed];
		taz = (u32) taz_spec_66[speed];
		trfs = (u32) trfs_spec_66[speed];
		tsr = (u32) tsr_spec_66[speed];
		tli = (u32) tli_maxspec_66[speed];
    } else if (mpc5xxx_clock_period == CLK_IPB132MHZ) {
    
    	t2cyc = (u32) t2cyc_spec_132[speed];
		tcyc = (u32) tcyc_spec_132[speed];
		tds = (u32) tds_spec_132[speed];
		tdh = (u32) tdh_spec_132[speed];
		tdvs = (u32) tdvs_spec_132[speed];
		tdvh = (u32) tdvh_spec_132[speed];
		tfs = (u32) tfs_minspec_132[speed];
		tmli = (u32) tmli_spec_132[speed];
		tenv = (u32) tenv_minspec_132[speed];
		tss = (u32) tss_spec_132[speed];
		trp = (u32) trp_spec_132[speed];
		tack = (u32) tack_spec_132[speed];
		tzah = (u32) tzah_spec_132[speed];
		taz = (u32) taz_spec_132[speed];
		trfs = (u32) trfs_spec_132[speed];
		tsr = (u32) tsr_spec_132[speed];
		tli = (u32) tli_maxspec_132[speed];

	}

	DPRINTK ("UDMA t2cyc = %d\n", t2cyc);
	DPRINTK ("UDMA tcyc  = %d\n", tcyc);
	DPRINTK ("UDMA tds   = %d\n", tds);
	DPRINTK ("UDMA tdh   = %d\n", tdh);
	DPRINTK ("UDMA tdvs  = %d\n", tdvs);
	DPRINTK ("UDMA tdvh  = %d\n", tdvh);
	DPRINTK ("UDMA tfs   = %d\n", tfs);
	DPRINTK ("UDMA tli   = %d\n", tli);
	DPRINTK ("UDMA tmli  = %d\n", tmli);
	DPRINTK ("UDMA taz   = %d\n", taz);
	DPRINTK ("UDMA tenv  = %d\n", tenv);
	DPRINTK ("UDMA tsr   = %d\n", tsr);
	DPRINTK ("UDMA tss   = %d\n", tss);
	DPRINTK ("UDMA trfs  = %d\n", trfs);
	DPRINTK ("UDMA trp   = %d\n", trp);
	DPRINTK ("UDMA tack  = %d\n", tack);
	DPRINTK ("UDMA tzah  = %d\n", tzah);
    
	mpc5xxx_timings[which].udma1 = (t2cyc << 24) | (tcyc << 16) | (tds << 8) | (tdh);
	mpc5xxx_timings[which].udma2 = (tdvs << 24) | (tdvh << 16) | (tfs << 8) | (tli);
	mpc5xxx_timings[which].udma3 = (tmli << 24) | (taz << 16) | (tenv << 8) | (tsr);
	mpc5xxx_timings[which].udma4 = (tss << 24) | (trfs << 16) | (trp << 8) | (tack);
	mpc5xxx_timings[which].udma5 = (tzah << 24);

	mpc5xxx_timings[which].using_udma = 1;
}

static void mpc5xxx_ide_calc_mdma_timings(ide_drive_t *drive, u8 speed)
{
	int which = drive->select.b.unit & 0x01;
	u32 t0M, td, tkw, tm, th, tj, tn;
	
	if (mpc5xxx_clock_period == CLK_IPB66MHZ) {

		t0M = (u32) t0M_spec_66[speed];
		td = (u32) td_spec_66[speed];
		tkw = (u32) tkw_spec_66[speed];
		tm = (u32) tm_spec_66[speed];
		th = (u32) th_spec_66[speed];
		tj = (u32) tj_spec_66[speed];
		tn = (u32) tn_spec_66[speed];

	} else if (mpc5xxx_clock_period == CLK_IPB132MHZ) {

		t0M = (u32) t0M_spec_132[speed];
		td = (u32) td_spec_132[speed];
		tkw = (u32) tkw_spec_132[speed];
		tm = (u32) tm_spec_132[speed];
		th = (u32) th_spec_132[speed];
		tj = (u32) tj_spec_132[speed];
		tn = (u32) tn_spec_132[speed];

	}

	DPRINTK ("t0M = %d\n", t0M);
	DPRINTK ("td  = %d\n", td);
	DPRINTK ("tkw = %d\n", tkw);
	DPRINTK ("tm  = %d\n", tm);
	DPRINTK ("th  = %d\n", th);
	DPRINTK ("tj  = %d\n", tj);
	DPRINTK ("tn  = %d\n", tn);

	mpc5xxx_timings[which].mdma1 = (t0M << 24) | (td << 16) | (tkw << 8) | (tm);
	mpc5xxx_timings[which].mdma2 = (th << 24) | (tj << 16) | (tn << 8);

	mpc5xxx_timings[which].using_udma = 0;
}

#endif /* MPC5xxx_IDE_HAS_DMA */

static int mpc5xxx_ide_speedproc(ide_drive_t *drive, u8 speed)
{
	if (speed > XFER_UDMA_2)
		speed = XFER_UDMA_2;

	switch(speed) {
#if MPC5xxx_IDE_HAS_DMA
	case XFER_UDMA_2:
	case XFER_UDMA_1:
	case XFER_UDMA_0:
		printk("%s: Setting UDMA %d timings\n", drive->name, speed - XFER_UDMA_0);
		mpc5xxx_ide_calc_udma_timings(drive, speed - XFER_UDMA_0);
		break;
	case XFER_MW_DMA_2:
	case XFER_MW_DMA_1:
	case XFER_MW_DMA_0:
		printk("%s: Setting MDMA %d timings\n", drive->name, speed - XFER_MW_DMA_0);
		mpc5xxx_ide_calc_mdma_timings(drive, speed - XFER_MW_DMA_0);
		break;
#endif /* MPC5xxx_IDE_HAS_DMA */
	case XFER_PIO_4:
	case XFER_PIO_3:
	case XFER_PIO_2:
	case XFER_PIO_0:
		break;
	default:
		return -EINVAL;
	}
	mpc5xxx_ide_tuneproc(drive, mpc5xxx_dma2pio(speed));
	return ide_config_drive_speed(drive, speed);
}

#if MPC5xxx_IDE_HAS_DMA

#ifdef DEBUG
static void dump_bds(void)
{
	TaskBD2_t *bd;
	int i;
	
	do {

		bd = (TaskBD2_t *)TaskGetBD(mpc5xxx_ata_sdma_task, i);
		if (bd)
			printk("bd%03d: %08lx %08lx %08lx\n", i, bd->Status, bd->DataPtr[0], bd->DataPtr[1]);
		i++;

	} while(bd);

}
#else
static inline void dump_bds(void) { }
#endif

static int mpc5xxx_ide_dma_check(ide_drive_t *drive)
{
	u8 speed = ide_dma_speed(drive, 1);

	DPRINTK("dma_check: speed=%x\n", speed);
	if (speed == 0)
		return -1;
	mpc5xxx_ide_speedproc(drive, speed);

	if (ide_dma_enable(drive))
		drive->using_dma = 1;
	else
		drive->using_dma = 0;

	return 0;
}

static int mpc5xxx_ide_continue_dma(ide_hwif_t *hwif)
{
	struct scatterlist *sg = hwif->sg_table + mpc5xxx_ata_dma_cur_sg;
	int ddir = hwif->sg_dma_direction;
	u32 cur_addr = mpc5xxx_ata_dma_cur_addr;
	u32 cur_len = mpc5xxx_ata_dma_cur_len;
	int next_bd;

	while (mpc5xxx_ata_dma_cur_sg < hwif->sg_nents && sg_dma_len(sg)) {
		if (cur_len == 0) {
			cur_addr = sg_dma_address(sg);
			cur_len = sg_dma_len(sg);
		}

		while (cur_len) {
			unsigned int tc;

			if (mpc5xxx_ata_free_bds == 0) {
				mpc5xxx_ata_dma_cur_len = cur_len;
				mpc5xxx_ata_dma_cur_addr = cur_addr;
				return 0;
			}
			tc  = (cur_len < 0xfe00) ? cur_len: 0xfe00;

			if (ddir == PCI_DMA_FROMDEVICE)
			{

				invalidate_dcache_range((u32)phys_to_virt(cur_addr), (u32)phys_to_virt(cur_addr)+(u32)tc);
				next_bd = TaskBDAssign(mpc5xxx_ata_sdma_task,
			       			     (void *)&mpc5xxx_ataregs->fifo_data,
			       			     (void *)cur_addr, tc, 0);
			} else {

				flush_dcache_range((u32)phys_to_virt(cur_addr), (u32)phys_to_virt(cur_addr)+(u32)tc);

				next_bd = TaskBDAssign(mpc5xxx_ata_sdma_task, (void *)cur_addr,
	       					     (void *)&mpc5xxx_ataregs->fifo_data, tc, 0);
			}
			DPRINTK("SDMA setup @%08x, l: %x, nextbd: %d !\n", cur_addr, tc, next_bd);
			if (mpc5xxx_ata_sdma_nextbd < 0)
				mpc5xxx_ata_sdma_nextbd = next_bd;
			cur_addr += tc;
			cur_len -= tc;
			mpc5xxx_ata_free_bds--;
		}
		sg++;
		mpc5xxx_ata_dma_cur_sg++;
	}

	return 0;
}

static int mpc5xxx_ide_build_dmatable(ide_drive_t *drive, struct request *rq, int ddir)
{
	ide_hwif_t *hwif = HWIF(drive);
	TaskSetupParamSet_t ata_setup;

	/* Build sglist */
	if (rq->cmd == IDE_DRIVE_TASKFILE)
		hwif->sg_nents = ide_raw_build_sglist(hwif, rq);
	else
		hwif->sg_nents = ide_build_sglist(hwif, rq, ddir);
	if (!hwif->sg_nents)
		return 0;
	
	/* Setup BestComm task */
	ata_setup.NumBD = MAX_DMA_BUFFERS;
	ata_setup.Size.MaxBuf = 0xfe00;
	if (ddir == PCI_DMA_FROMDEVICE) {
		ata_setup.Initiator = INITIATOR_ATA_RX;
		ata_setup.StartAddrDst = 0;
		ata_setup.StartAddrSrc = (u32)&mpc5xxx_ataregs->fifo_data;
		ata_setup.IncrDst = 4;
		ata_setup.IncrSrc = 0;
		ata_setup.SzDst = 4;
		ata_setup.SzSrc = 4;
	} else {
		ata_setup.Initiator = INITIATOR_ATA_TX;
		ata_setup.StartAddrDst = (u32)&mpc5xxx_ataregs->fifo_data;
		ata_setup.StartAddrSrc = 0;
		ata_setup.IncrDst = 0;
		ata_setup.IncrSrc = 4;
		ata_setup.SzDst = 4;
		ata_setup.SzSrc = 4;
	}

	mpc5xxx_ata_sdma_task = TaskSetup (TASK_ATA, &ata_setup);
	DPRINTK("Task setup, src: %08lx, dst: %08lx, nr_sectors: %ld -> %d\n",
	       ata_setup.StartAddrSrc, ata_setup.StartAddrDst, rq->nr_sectors,
	       mpc5xxx_ata_sdma_task);


	mpc5xxx_ata_dma_cur_sg = 0;
	mpc5xxx_ata_dma_cur_addr = 0;
	mpc5xxx_ata_dma_cur_len = 0;
	mpc5xxx_ata_free_bds = MAX_DMA_BUFFERS;
	mpc5xxx_ata_sdma_nextbd = -1;

	if (mpc5xxx_ide_continue_dma(hwif) == 0) {
		dump_bds();
		return 1;
	}

	pci_unmap_sg(hwif->pci_dev,
		     hwif->sg_table,
		     hwif->sg_nents,
		     hwif->sg_dma_direction);
	hwif->sg_dma_active = 0;
	return 0; /* revert to PIO for this request */
}

#if 0
/*
 * From experimentation, it seems that the BestComm task for ATA will
 * clear the Lenght (Status) of a BD that has been processed, we base
 * ourselves on this behaviour here.
 * Note that it seem also to interrupt us only after sending all 4 BDs
 * which is sub-optimal imho, 3 BDs would have been a better threshold
 * as it allows us to fill new ones while the last one is still beeing
 * processed. Maybe it's just that the disk is too fast though... It
 * would then be worth considering:
 * - turning the ATA task into a single ptr BD (double ptr BD is useless
 *   waste of memory in our case unless we want to use DMA for ATAPI
 *   command writes, but the kernel IDE layer isn't too much ready for
 *   us to do that in a single operation)
 * - increasing the max amount of BDs the ATA task may use
 */
#undef COUNT_BUFFS_PER_IRQ

static void mpc5xxx_ide_sdma_irq(int irq, void *dev_id, struct pt_regs * regs)
{
	ide_hwif_t *hwif = dev_id;
#ifdef COUNT_BUFFS_PER_IRQ
	int i = 0;
#endif

	DPRINTK("SDMA irq\n");
	dump_bds();
	do {
		TaskBD2_t *bd = (TaskBD2_t *)TaskGetBD(mpc5xxx_ata_sdma_task, mpc5xxx_ata_sdma_nextbd);
		if (bd->Status != 0)
			break;
		TaskBDRelease (mpc5xxx_ata_sdma_task);
		DPRINTK("released: %d\n", mpc5xxx_ata_sdma_nextbd);
		mpc5xxx_ata_sdma_nextbd = (mpc5xxx_ata_sdma_nextbd + 1) % MAX_DMA_BUFFERS;
		mpc5xxx_ata_free_bds++;
#ifdef COUNT_BUFFS_PER_IRQ
		i++;
#endif
	} while(mpc5xxx_ata_free_bds < MAX_DMA_BUFFERS);
#ifdef COUNT_BUFFS_PER_IRQ
	if (i < 4)
		printk("<%d>\n", i);
#endif
	mpc5xxx_ide_continue_dma(hwif);

	dump_bds();
}
#endif

static int mpc5xxx_ide_dma_read(ide_drive_t *drive)
{
	struct request *rq = HWGROUP(drive)->rq;
	int which = drive->select.b.unit & 0x01;
	u8 lba48 = (drive->addressing == 1) ? 1 : 0;
	task_ioreg_t command = WIN_NOP;
	u8 dma_mode = MPC5xxx_ATA_DMAMODE_IE | MPC5xxx_ATA_DMAMODE_READ | MPC5xxx_ATA_DMAMODE_FE;

	if (!mpc5xxx_ide_build_dmatable(drive, rq, PCI_DMA_FROMDEVICE))
		/* try PIO instead of DMA */
		return 1;

	/* Setup FIFO if direction changed */
	if (mpc5xxx_ata_dma_last_write) {
		mpc5xxx_ata_dma_last_write = 0;
		WAIT_TIP_BIT_CLEAR_NEW(IDE_COMMAND_REG);
		out_8(&mpc5xxx_ataregs->dma_mode, MPC5xxx_ATA_DMAMODE_FR);
		/* Configure it with granularity to 7 like sample code */
		out_8(&mpc5xxx_ataregs->fifo_control, 7);
		out_be16(&mpc5xxx_ataregs->fifo_alarm, 128);
	}
	if (mpc5xxx_timings[which].using_udma)
		dma_mode |= MPC5xxx_ATA_DMAMODE_UDMA;

	WAIT_TIP_BIT_CLEAR_NEW(IDE_COMMAND_REG);
	out_8(&mpc5xxx_ataregs->dma_mode, dma_mode);

	/*DPRINTK("SDMA starting read (dmamode: %x lba48: %d)\n", dma_mode, lba48);*/

	/* Start DMA. XXX FIXME: See if we can move that to ide_dma_begin */
	TaskStart(mpc5xxx_ata_sdma_task, TASK_AUTOSTART_ENABLE, mpc5xxx_ata_sdma_task, TASK_INTERRUPT_ENABLE);

	drive->waiting_for_dma = 1;

	if (drive->media != ide_disk)
		return 0;

	command = (lba48) ? WIN_READDMA_EXT : WIN_READDMA;
	if (rq->cmd == IDE_DRIVE_TASKFILE) {
		ide_task_t *args = rq->special;
		command = args->tfRegister[IDE_COMMAND_OFFSET];
	}

	WAIT_TIP_BIT_CLEAR_NEW(IDE_COMMAND_REG);

	 /* issue cmd to drive */
	ide_execute_command(drive, command, &ide_dma_intr, 2*WAIT_CMD, NULL);

	return HWIF(drive)->ide_dma_begin(drive);
}

static int mpc5xxx_ide_dma_write(ide_drive_t *drive)
{
	struct request *rq = HWGROUP(drive)->rq;
	int which = drive->select.b.unit & 0x01;
	u8 lba48 = (drive->addressing == 1) ? 1 : 0;
	task_ioreg_t command = WIN_NOP;
	u8 dma_mode = MPC5xxx_ATA_DMAMODE_IE | MPC5xxx_ATA_DMAMODE_WRITE;

	if (!mpc5xxx_ide_build_dmatable(drive, rq, PCI_DMA_TODEVICE))
		/* try PIO instead of DMA */
		return 1;

	/* Setup FIFO if direction changed */
	if (!mpc5xxx_ata_dma_last_write) {
		mpc5xxx_ata_dma_last_write = 1;
		WAIT_TIP_BIT_CLEAR_NEW(IDE_COMMAND_REG);
		/* Configure FIFO with granularity to 4 like sample code */
		out_8(&mpc5xxx_ataregs->fifo_control, 4);
		/*out_be16(&mpc5xxx_ataregs->fifo_alarm, 256);*/
		out_be16(&mpc5xxx_ataregs->fifo_alarm, 384);
	}

	if (mpc5xxx_timings[which].using_udma)
		dma_mode |= MPC5xxx_ATA_DMAMODE_UDMA;

	WAIT_TIP_BIT_CLEAR_NEW(IDE_COMMAND_REG);
	out_8(&mpc5xxx_ataregs->dma_mode, dma_mode);

	DPRINTK("SDMA starting write (dmamode: %x lba48: %d)\n", dma_mode, lba48);

	/* Start DMA. XXX FIXME: See if we can move that to ide_dma_begin */
	TaskStart(mpc5xxx_ata_sdma_task, TASK_AUTOSTART_ENABLE, mpc5xxx_ata_sdma_task, TASK_INTERRUPT_ENABLE);

	drive->waiting_for_dma = 1;

	if (drive->media != ide_disk)
		return 0;

	command = (lba48) ? WIN_WRITEDMA_EXT : WIN_WRITEDMA;
	if (rq->cmd == IDE_DRIVE_TASKFILE) {
		ide_task_t *args = rq->special;
		command = args->tfRegister[IDE_COMMAND_OFFSET];
	}

	WAIT_TIP_BIT_CLEAR_NEW(IDE_COMMAND_REG);

	 /* issue cmd to drive */
	ide_execute_command(drive, command, &ide_dma_intr, 2*WAIT_CMD, NULL);

	return HWIF(drive)->ide_dma_begin(drive);
}

static int mpc5xxx_ide_dma_count(ide_drive_t *drive)
{
	return HWIF(drive)->ide_dma_begin(drive);
}

static int mpc5xxx_ide_dma_begin(ide_drive_t *drive)
{
	return 0;
}

static int mpc5xxx_ide_dma_end(ide_drive_t *drive)
{
	DPRINTK("SDMA end\n");

	/* XXX TEST */
	//	out_8(&mpc5xxx_ataregs->dma_mode, MPC5xxx_ATA_DMAMODE_FR | MPC5xxx_ATA_DMAMODE_HUT);
	
	do {
		TaskBD2_t *bd = (TaskBD2_t *)TaskGetBD(mpc5xxx_ata_sdma_task, mpc5xxx_ata_sdma_nextbd);
		if (bd->Status != 0)
			printk(KERN_WARNING "%s: BD non free on command completion !\n", drive->name);
		TaskBDRelease (mpc5xxx_ata_sdma_task);
		mpc5xxx_ata_free_bds++;
		mpc5xxx_ata_sdma_nextbd = (mpc5xxx_ata_sdma_nextbd + 1) % MAX_DMA_BUFFERS;
	} while(mpc5xxx_ata_free_bds < MAX_DMA_BUFFERS);

	/*TaskBDReset(mpc5xxx_ata_sdma_task);*/
	TaskStop(mpc5xxx_ata_sdma_task);
	TaskBDReset(mpc5xxx_ata_sdma_task);
	drive->waiting_for_dma = 0;

	return 0;
}

static int mpc5xxx_ide_dma_test_irq(ide_drive_t *drive)
{
#if 1
	unsigned int timeout=0;
	while ((in_8(&mpc5xxx_ataregs->fifo_status) & MPC5xxx_ATA_FIFOSTAT_EMPTY) == 0) {
		if (timeout++ > 100000) {
			printk("ATA fifo empty timeout\n");

			printk("host_status: %08x\n", mpc5xxx_ataregs->host_status);
			printk("fifo_status: %02x\n", mpc5xxx_ataregs->fifo_control);
			printk("fifo_alarm: %04x\n",  mpc5xxx_ataregs->fifo_alarm);
			printk("fifo_rdp: %04x\n",    mpc5xxx_ataregs->fifo_rdp);
			printk("fifo_wrp: %04x\n",    mpc5xxx_ataregs->fifo_wrp);
			printk("tf_control: %02x\n",  mpc5xxx_ataregs->tf_control);
			printk("tf_features: %02x\n", mpc5xxx_ataregs->tf_features);
			printk("tf_command: %02x\n",  mpc5xxx_ataregs->tf_command);
			printk("dma_mode: %02x\n",    mpc5xxx_ataregs->dma_mode);

			dump_bds();
			return 1;
		}
		/*usleep(10);*/
		udelay(10);
	}
	return 1;
#else
	return (drive->waiting_for_dma);
#endif
}

static int mpc5xxx_ide_dma_host_off(ide_drive_t *drive)
{
	return 0;
}

static int mpc5xxx_ide_dma_host_on(ide_drive_t *drive)
{
	return 0;
}

static inline void mpc5xxx_ide_toggle_bounce(ide_drive_t *drive, int on)
{
	u64 addr = BLK_BOUNCE_HIGH;	/* dma64_addr_t */

	if (on && drive->media == ide_disk)
		addr = 0xfffffffful;
	blk_queue_bounce_limit(&drive->queue, addr);
}

static int mpc5xxx_ide_dma_off_quietly (ide_drive_t *drive)
{
	drive->using_dma = 0;
	mpc5xxx_ide_toggle_bounce(drive, 0);
	return HWIF(drive)->ide_dma_host_off(drive);
}

int mpc5xxx_ide_dma_on (ide_drive_t *drive)
{
	drive->using_dma = 1;
	mpc5xxx_ide_toggle_bounce(drive, 1);
	return HWIF(drive)->ide_dma_host_on(drive);
}

static int mpc5xxx_ide_dma_lostirq(ide_drive_t *drive)
{
	printk(KERN_ERR "%s: lost interrupt\n", drive->name);
	dump_bds();
	return 0;
}

#endif /* MPC5xxx_IDE_HAS_DMA */


ide_ioreg_t mpc5xxx_ide_get_base(int index)
{
	if (mpc5xxx_ataregs == NULL || index != mpc5xxx_hwif_index)
		return 0;
	return (ide_ioreg_t)&mpc5xxx_ataregs->tf_data;
}

void mpc5xxx_ide_init_hwif_ports(hw_regs_t *hw,
				 ide_ioreg_t data_port, ide_ioreg_t ctrl_port,
				 int *irq)
{
	int i;

	if (data_port == 0)
		return;

	if (data_port != (ide_ioreg_t)&mpc5xxx_ataregs->tf_data) {
		/* Probably a PCI interface... */
		for (i = IDE_DATA_OFFSET; i <= IDE_STATUS_OFFSET; ++i)
			hw->io_ports[i] = data_port + i - IDE_DATA_OFFSET;
		hw->io_ports[IDE_CONTROL_OFFSET] = ctrl_port;
		return;
	}

       	hw->io_ports[IDE_DATA_OFFSET] = (ide_ioreg_t)&mpc5xxx_ataregs->tf_data;
	hw->io_ports[IDE_ERROR_OFFSET] = (ide_ioreg_t)&mpc5xxx_ataregs->tf_features;
       	hw->io_ports[IDE_NSECTOR_OFFSET] = (ide_ioreg_t)&mpc5xxx_ataregs->tf_sec_count;
       	hw->io_ports[IDE_SECTOR_OFFSET] = (ide_ioreg_t)&mpc5xxx_ataregs->tf_sec_num;
       	hw->io_ports[IDE_LCYL_OFFSET] = (ide_ioreg_t)&mpc5xxx_ataregs->tf_cyl_low;
       	hw->io_ports[IDE_HCYL_OFFSET] = (ide_ioreg_t)&mpc5xxx_ataregs->tf_cyl_high;
       	hw->io_ports[IDE_SELECT_OFFSET] = (ide_ioreg_t)&mpc5xxx_ataregs->tf_dev_head;
       	hw->io_ports[IDE_STATUS_OFFSET] = (ide_ioreg_t)&mpc5xxx_ataregs->tf_command;
       	hw->io_ports[IDE_CONTROL_OFFSET] = (ide_ioreg_t)&mpc5xxx_ataregs->tf_control;

	if (irq)
		*irq = MPC5xxx_ATA_IRQ;
}

static int mpc5xxx_ide_setup(void)
{
	struct mpc5xxx_gpio *gpio_regs = (struct mpc5xxx_gpio *)MPC5xxx_GPIO;
	struct mpc5xxx_sdma *sdma_regs = (struct mpc5xxx_sdma *)MPC5xxx_SDMA;
	u32 reg;
#ifdef CONFIG_UBOOT
	extern unsigned char __res[];
	bd_t *bd = (bd_t *)__res;
#define MPC5xxx_IPBFREQ bd->bi_ipbfreq
#else
#define MPC5xxx_IPBFREQ CONFIG_PPC_5xxx_IPBFREQ
#endif

	/*
	 * Check port configuration & enable IDE chip selects
	 */
	reg = in_be32(&gpio_regs->port_config);
	printk(/*KERN_DEBUG*/ "Port Config is: 0x%08x\n", reg);
	reg = (reg & 0xfcfffffful) | 0x01000000ul;
	out_be32(&gpio_regs->port_config, reg);

	/*
	 * All sample codes do that...
	 */
	out_be32(&mpc5xxx_ataregs->share_cnt, 0);

	/*
	 * Configure & reset host
	 */
	out_be32(&mpc5xxx_ataregs->config, MPC5xxx_ATA_HOSTCONF_IE | MPC5xxx_ATA_HOSTCONF_IORDY
					| MPC5xxx_ATA_HOSTCONF_SMR | MPC5xxx_ATA_HOSTCONF_FR);
	udelay(10);
	out_be32(&mpc5xxx_ataregs->config, MPC5xxx_ATA_HOSTCONF_IE | MPC5xxx_ATA_HOSTCONF_IORDY);	

	/*
	 * check ip bus frequency
	 */
	if(MPC5xxx_IPBFREQ/1000000 == 66) {

		mpc5xxx_clock_period = CLK_IPB66MHZ;
		out_be32 (&mpc5xxx_ataregs->share_cnt, 0x00800000);

	} else {

		mpc5xxx_clock_period = CLK_IPB132MHZ;
		out_be32 (&mpc5xxx_ataregs->share_cnt, 0x01000000);

	}

	printk("ipb=%dMHz, set clock period to %d\n",(int)(MPC5xxx_IPBFREQ/1000000), 
		mpc5xxx_clock_period);

	/*
	 * Disable prefetch on commbus
	 */
	out_be16(&sdma_regs->PtdCntrl, in_be16(&sdma_regs->PtdCntrl) | 0x0001);

	mpc5xxx_timings[0].pio1 = 0x100a0a00;
	mpc5xxx_timings[0].pio2 = 0x02040600;
	mpc5xxx_timings[1].pio1 = 0x100a0a00;
	mpc5xxx_timings[1].pio2 = 0x02040600;
       	out_be32(&mpc5xxx_ataregs->pio1, 0x100a0a00);
       	out_be32(&mpc5xxx_ataregs->pio2, 0x02040600);
     
	printk("GPIO config: %08x\n", in_be32((u32 *)0xf0000b00));
	printk("ATA invalid: %08x\n", in_be32((u32 *)0xf0003a2c));
	printk("ATA hostcnf: %08x\n", in_be32((u32 *)0xf0003a00));
	printk("ATA pio1   : %08x\n", in_be32((u32 *)0xf0003a08));
	printk("ATA pio2   : %08x\n", in_be32((u32 *)0xf0003a0c));
	printk("XLB Arb cnf: %08x\n", in_be32((u32 *)0xf0001f40));

	return 0;
}

void mpc5xxx_ide_probe(void)
{
	int i;
	ide_hwif_t *hwif;
#if MPC5xxx_IDE_HAS_DMA
	static TaskSetupParamSet_t ata_setup;
#endif

	for (i = 0; i < MAX_HWIFS && ide_hwifs[0].io_ports[IDE_DATA_OFFSET] != 0;)
		i++;
	if (i >= MAX_HWIFS) {
		printk(KERN_ERR "mpc5xxx_ide: No free hwif slot !\n");
		return;
	}

	mpc5xxx_hwif_index = i;
	mpc5xxx_ataregs = (struct mpc5xxx_ata *)MPC5xxx_ATA;

	mpc5xxx_xlbregs = (struct mpc5xxx_xlb *)MPC5xxx_XLB;

#if XLB_PRIO_ON
	mpc5xxx_xlbregs ->master_priority = XLB_PRIO_CORE
                   | (XLB_PRIO_USB << 4)
                   | (XLB_PRIO_BC << 8)
                   | (XLB_PRIO_PCI << 12);
	mpc5xxx_xlbregs ->master_pri_enable = 0x07;
#endif

	if (mpc5xxx_ide_setup()) {
		printk(KERN_ERR "mpc5xxx_ide: Setting up interface failed !\n");
		return;
	}

	printk("mpc5xxx_ide: Setting up IDE interface ide%d...\n", i);

	hwif = &ide_hwifs[i];
	setup_hwif_mpc5xxx_iops(hwif);
	hwif->mmio = 2;
	hwif->irq = MPC5xxx_ATA_IRQ;
	mpc5xxx_ide_init_hwif_ports(&hwif->hw,
				  (ide_ioreg_t)&mpc5xxx_ataregs->tf_data, 0, &hwif->irq);
	memcpy(hwif->io_ports, hwif->hw.io_ports, sizeof(hwif->io_ports));
	hwif->chipset = ide_mpc5xxx;
	hwif->noprobe = 0;
	hwif->udma_four = 0;
	hwif->addressing = 1; /* LBA48 not supported */
	hwif->drives[0].unmask = hwif->drives[1].unmask = 1;
	hwif->tuneproc = mpc5xxx_ide_tuneproc;
	hwif->speedproc = mpc5xxx_ide_speedproc;
	hwif->selectproc = mpc5xxx_ide_apply_timings;
	hwif->atapi_dma = 0;
	hwif->swdma_mask = 0x00;
	hwif->mwdma_mask = 0x00;
	hwif->ultra_mask = 0x00;

#if MPC5xxx_IDE_HAS_DMA
	/*
	 * Setup DMA if enabled. We can "hijack" hwif->sg_table and hwif->dma_table_cpu
	 * provided that we leave hwif->dma_base clear. If we don't leave it clear, the
	 * ide common code will try to use it's own free() routines on these and will
	 * break as it assumes standard PRD table.
	 */
	hwif->sg_table = kmalloc(sizeof(struct scatterlist) * MAX_DMA_BUFFERS, GFP_KERNEL);
	if (hwif->sg_table == NULL)
		goto no_dma;
   
	/*
	 * Setup dummy ATA task for writing so we get the task number for requesting
	 * the interrupt
	 */
	mpc5xxx_bestcomm_init();

	ata_setup.NumBD = MAX_DMA_BUFFERS;
	ata_setup.Size.MaxBuf = 0xffe0;
	ata_setup.Initiator = INITIATOR_ATA_TX;
	ata_setup.StartAddrDst = (u32)&mpc5xxx_ataregs->fifo_data;
	ata_setup.StartAddrSrc = 0;
	ata_setup.IncrDst = 0;
	ata_setup.IncrSrc = 4;
	ata_setup.SzDst = 4;
	ata_setup.SzSrc = 4;
	mpc5xxx_ata_sdma_task = TaskSetup (TASK_ATA, &ata_setup);
	mpc5xxx_ata_dma_last_write = 1;

	printk("ATA DMA task: %d\n", mpc5xxx_ata_sdma_task);

#if 0
	if (request_irq(MPC5xxx_SDMA_IRQ_BASE + mpc5xxx_ata_sdma_task, mpc5xxx_ide_sdma_irq,
			SA_INTERRUPT, "ide dma", hwif)) {
		printk(KERN_ERR "mpc5xxx_ide: SDMA irq allocation failed\n");
		goto no_dma;
	}
#endif
	hwif->atapi_dma = 1;
	hwif->autodma = 1;
#ifdef CONFIG_BLK_DEV_IDE_MPC5xxx_MDMA
	hwif->mwdma_mask = 0x07;
#endif
#ifdef CONFIG_BLK_DEV_IDE_MPC5xxx_UDMA
	hwif->ultra_mask = 0x07;
#endif
	hwif->ide_dma_off = &__ide_dma_off;
	hwif->ide_dma_off_quietly = &mpc5xxx_ide_dma_off_quietly;
	hwif->ide_dma_on = &mpc5xxx_ide_dma_on;
	hwif->ide_dma_check = &mpc5xxx_ide_dma_check;
	hwif->ide_dma_read = &mpc5xxx_ide_dma_read;
	hwif->ide_dma_write = &mpc5xxx_ide_dma_write;
	hwif->ide_dma_count = &mpc5xxx_ide_dma_count;
	hwif->ide_dma_begin = &mpc5xxx_ide_dma_begin;
	hwif->ide_dma_end = &mpc5xxx_ide_dma_end;
	hwif->ide_dma_test_irq = &mpc5xxx_ide_dma_test_irq;
	hwif->ide_dma_host_off = &mpc5xxx_ide_dma_host_off;
	hwif->ide_dma_host_on = &mpc5xxx_ide_dma_host_on;
	hwif->ide_dma_good_drive = &__ide_dma_good_drive;
	hwif->ide_dma_bad_drive = &__ide_dma_bad_drive;
	hwif->ide_dma_verbose = &__ide_dma_verbose;
	hwif->ide_dma_timeout = &__ide_dma_timeout;
	hwif->ide_dma_retune = &__ide_dma_retune;
	hwif->ide_dma_lostirq = &mpc5xxx_ide_dma_lostirq;
 no_dma:
#endif /* MPC5xxx_IDE_HAS_DMA */
	;
}
