#include <linux/module.h>
#include <linux/moduleparam.h>
#include <linux/init.h>

#include <linux/sched.h>
#include <linux/kernel.h>	/* printk() */
#include <linux/slab.h>		/* kmalloc() */
#include <linux/fs.h>		/* everything... */
#include <linux/errno.h>	/* error codes */
#include <linux/timer.h>
#include <linux/types.h>	/* size_t */
#include <linux/fcntl.h>	/* O_ACCMODE */
#include <linux/hdreg.h>	/* HDIO_GETGEO */
#include <linux/kdev_t.h>
#include <linux/vmalloc.h>
#include <linux/genhd.h>
#include <linux/blkdev.h>
#include <linux/buffer_head.h>	/* invalidate_bdev */
#include <linux/bio.h>

#include <linux/irq.h>
#include <linux/interrupt.h>

#include <linux/slab.h>
#include <linux/mm.h>
#include <asm/io.h>
#include <asm/arch/hardware.h>
#include <asm/cacheflush.h>

#include <linux/suspend.h>
#include <linux/kthread.h>
#include <asm/atomic.h>


extern void __udelay(unsigned int);
#define udelay(x) __udelay((x))


/* After this much idle time, the driver will simulate a media change. */
#define INVALIDATE_DELAY	30*HZ

/* The kernel talks to us in terms of small sectors, always. */
#define KERNEL_SECTOR_SIZE	512

#define SD_DEFNAME			"tssdcard"
#define SD_DEFIOSIZE		4
#define SD_BLKSIZE			1024  //nsectors?
#define SD_HARDSECT			512
#define SD_SHIFT			4  //max 16 partitions 2^4
#define SD_NUMPART			1 << 4

#define TSSDCARD_MINORS		SD_NUMPART
#define MINOR_SHIFT			SD_SHIFT
#define DEVNUM(kdevnum)		(MINOR(kdev_t_to_nr(kdevnum)) >> MINOR_SHIFT

#ifdef CONFIG_MACH_TS72XX
#include "tssdcore.c"

#define DMA_ENABLE 1	
#define THREAD_ENABLE 1	        	

#define DMA_CHANNEL			10
#define DMA_BUFSIZE			1024
#define DMA_M2M_0_SIZE		0x40
#define DMA_M2M_1_SIZE		0x40

#define TS7XXX_SDCARD1		0x13000000
#define TS7XXX_SDCARD2		0x72000020

#ifndef SYSCON_OFFSET
#define SYSCON_OFFSET 0x130000
#define SYSCON_BASE (0x80800000|SYSCON_OFFSET)
#define SYSCON_SCRREG0 0x0040
#define SYSCON_SCRREG1 0x0044
#define SYSCON_SWLOCK 0x00C0
#define SYSCON_PWRCNT 0x0004
#define SYSCON_DEVCFG 0x0080
#define SYSCON_PWRCNT_DMA_M2MCH0 0x04000000
#define SYSCON_PWRCNT_DMA_M2MCH1 0x08000000
#define SYSCON_DEVCFG_D0onG 0x20000000
#define SYSCON_DEVCFG_D1onG 0x40000000
#endif

#ifndef DMA_OFFSET
#define DMA_OFFSET 0x000000
#define DMA_BASE (0x80000000|DMA_OFFSET)
#define DMAMM_0_CONTROL (DMA_BASE+0x0100)
#define DMAMM_1_CONTROL (DMA_BASE+0x0140)
#define DMA_M2M_0_BASE DMAMM_0_CONTROL
#define DMA_M2M_1_BASE DMAMM_1_CONTROL
#define M2M_OFFSET_CONTROL 0x0000
#define M2M_OFFSET_INTERRUPT 0x0004
#define M2M_OFFSET_STATUS 0x000C
#define M2M_OFFSET_BCR0 0x0010
#define M2M_OFFSET_BCR1 0x0014
#define M2M_OFFSET_SAR_BASE0 0x0018
#define M2M_OFFSET_SAR_BASE1 0x001C
#define M2M_OFFSET_SAR_CURRENT0 0x0024
#define M2M_OFFSET_SAR_CURRENT1 0x0028
#define M2M_OFFSET_DAR_BASE0 0x002C
#define M2M_OFFSET_DAR_BASE1 0x0030
#define M2M_OFFSET_DAR_CURRENT0 0x0034
#define M2M_OFFSET_DAR_CURRENT1 0x003C
#endif

#define NDEVICES 1

#endif
  
#ifdef CONFIG_TS7800_PLATFORM

#include "tssdcore2.c"

#define DMA_ENABLE 0	
#define THREAD_ENABLE 1	        	

#define DMA_CHANNEL			0

#define TS7800_IRQ_DMA		65
#define TS7800_IRQ_WAIT		66

#define TS7XXX_SDCARD1		0xE8000100
#define TS7800_DMAADDR		0xE8000400

#define TS7800_FPGASDDAT	0x80000104
#define TS7800_CPUDOORBELL	0xF1020400
#define TS7800_FPGADOORBELL	0xE8000200
#define TS7800_FPGAMSIMASK	0xE8000204
#define TS7800_FPGAIRQRAW	0xE8000208

#define DOORBELL_DMABIT		0x1 << 1
#define DOORBELL_WAITBIT	0x1 << 2

//TS-7800 ndevices=2 for Micro and Full size sockets
#define NDEVICES 2

#endif



enum {
  RM_SIMPLE  = 0,  /* The extra-simple request function */
  RM_FULL    = 1,  /* The full-blown version */
  RM_NOQUEUE = 2,  /* Use make_request */
};

static unsigned char *name = SD_DEFNAME;
static unsigned int io = TS7XXX_SDCARD1;
static int dmaenable = DMA_ENABLE;
static int threnable = THREAD_ENABLE;
static int dmach = DMA_CHANNEL;
static int reqmode = RM_FULL;
static int tssdcard_major = 0;
static int nsectors = SD_BLKSIZE;
static int ndevices = NDEVICES;

MODULE_AUTHOR("Technologic Systems");
MODULE_LICENSE("Proprietary");
//MODULE_LICENSE("GPL");
module_param(name, charp, 0);
module_param(io, int, 0);
module_param(dmaenable, int, 0);
module_param(threnable, int, 0);
module_param(dmach, int, 0);
module_param(reqmode, int, 0);

static int debug = 0;
module_param(debug, int, 0);

struct tssdcard_dev {
        struct sdcore tssdcore;

        char *devname;
		int size;                       /* Device size in sectors */

        struct gendisk *gd;             /* The gendisk structure */

        struct timer_list timer;

        short media_change;             /* Flag a media change? */

        int irq, irq_wait;
        unsigned long dmaregs;
        int dmach;
		
        atomic_t users;                    /* How many users */
        atomic_t busy;
		int parksect;

        struct request_queue *queue;    /* The device request queue */
		struct bio *bio;
        struct bio *biotail;
		
        struct task_struct *thread;
		
        wait_queue_head_t event;
        wait_queue_head_t qdma;
        wait_queue_head_t qwait;

        spinlock_t lock;                
		
};

static struct tssdcard_dev *devices = NULL;
static struct block_device_operations tssdcard_ops;

#define DMA_DECLBASE(x)	unsigned long dmaregs = (x);
#ifdef CONFIG_MACH_TS72XX
#define DMA_WRITE(x, y)	*(volatile uint32_t *)(dmaregs + M2M_OFFSET_ ## x) = (y) 
#define DMA_READ(x) *(volatile uint32_t *)(dmaregs + M2M_OFFSET_ ## x)
#endif
#ifdef CONFIG_TS7800_PLATFORM
#endif

static irqreturn_t int_handler(int irq, void *vidp, struct pt_regs regs)
{
  struct tssdcard_dev *dev = (struct tssdcard_dev *)vidp;
  
  //BUG_ON(irq != dev->irq || irq != dev->irq_wait);
  //printk("irq %d\n",irq);
  
  if (irq == dev->irq)
    wake_up_interruptible(&dev->qdma);
  else if (irq == dev->irq_wait)
    wake_up_interruptible(&dev->qwait);  
	
  disable_irq_nosync(irq);
	
  return IRQ_HANDLED;
}

pte_t * vmalloc_to_pte(void * vmalloc_addr)
{
  unsigned long addr = (unsigned long) vmalloc_addr;
  //volatile int *v = *(int *)vmalloc_addr;
  pmd_t *pmd;
  pte_t *pte = NULL;
  pgd_t *pgd;

  pgd = pgd_offset_k(addr);
  if (!pgd_none(*pgd)) {
    pmd = pmd_offset(pgd, addr);
    if (!pmd_none(*pmd)) {
      pte = pte_offset_kernel(pmd, addr);
    }
  }
  return pte;
}

#ifdef CONFIG_TS7800_PLATFORM
static void __attribute__((naked))
xdma_clean_range(char *start, char *end)
{
	asm volatile (
        "bic     r0, r0, #31						\n\
1:      mcr     p15, 0, r0, c7, c10, 1          @ clean D entry		\n\
        add     r0, r0, #32						\n\
        cmp     r0, r1							\n\
        blo     1b							\n\
        mcr     p15, 0, r0, c7, c10, 4          @ drain WB		\n\
        mov     pc, lr"
	::);
}

static void __attribute__((naked))
xdma_inv_range(char * start, char * end)
{
	asm volatile (
        "tst     r0, #31						\n\
        mcrne   p15, 0, r0, c7, c10, 1          @ clean D entry		\n\
        tst     r1, #31							\n\
        mcrne   p15, 0, r1, c7, c10, 1          @ clean D entry		\n\
        bic     r0, r0, #31 						\n\
1:      mcr     p15, 0, r0, c7, c6, 1           @ invalidate D entry	\n\
        add     r0, r0, #32						\n\
        cmp     r0, r1							\n\
        blo     1b							\n\
        mcr     p15, 0, r0, c7, c10, 4          @ drain WB		\n\
        mov     pc, lr"
	::);
}

static void tssdcard_irqwait(void *os_arg, int debug)
{
  struct tssdcard_dev *dev = (struct tssdcard_dev *)os_arg;
  unsigned long i=0, msireg;


  msireg = readl(TS7800_FPGAMSIMASK); //save msi 
  writel ( (~DOORBELL_WAITBIT), TS7800_CPUDOORBELL); //clear waitbit on doorbell
  writel ( (msireg | DOORBELL_WAITBIT), TS7800_FPGAMSIMASK); //write/mask waitbit on msi, ready to pool  
  while ( !(readl(TS7800_CPUDOORBELL)&DOORBELL_WAITBIT) ) //pooling doorbell reg until waitbit is set
  {
    //printk("irqwait waiting %d\n", readl(TS7800_CPUDOORBELL)) ;	
    i++;
	if (i<0) {
	
      DEFINE_WAIT(wait);
      prepare_to_wait(&dev->qwait, &wait, TASK_INTERRUPTIBLE);
	  //printk("sleeping wait\n");	  	  
      request_irq(dev->irq_wait, int_handler, SA_INTERRUPT|SA_SHIRQ, dev->devname, dev);
      if (!(readl(TS7800_CPUDOORBELL)&DOORBELL_WAITBIT))
        schedule();
      finish_wait(&dev->qwait, &wait);
      free_irq(dev->irq_wait, dev);
	  //printk("wokeup wait\n");	  
	  return;

    }
  }
  //printk("irqwait done %d\n", readl(TS7800_CPUDOORBELL)) ;	
  writel( (msireg & ~DOORBELL_WAITBIT), TS7800_FPGAMSIMASK); //restore/unmask waitbit on msi reg    
  
  return;  
}
#endif

static int tssdcard_dmastream(void *os_arg, unsigned char *dat, unsigned int buflen)
{
  struct tssdcard_dev *dev = (struct tssdcard_dev *)os_arg;
  static unsigned char dummymem[8];
  static unsigned long dummymem_paddr = 0;
  
#ifdef CONFIG_MACH_TS72XX 
  unsigned long dmaregs = dev->dmaregs;
	
  //udelay(1000); 
  //if (debug) printk("dmastream buflen:%d\n", buflen);
  
  DMA_WRITE(CONTROL, 0x11085000);
  //DMA_READ(CONTROL);
  
  DMA_WRITE(SAR_BASE0, io|SDDAT );	
	
  if (dat) {
    DMA_WRITE(DAR_BASE0, virt_to_bus(dat));
  }
  else {
    if (dummymem_paddr == 0) {
      pte_t *pte = vmalloc_to_pte(dummymem);
      dummymem_paddr = pte_pfn(*pte) << PAGE_SHIFT;
      dummymem_paddr += (unsigned long)dummymem & 0xfff;
    }
    DMA_WRITE(DAR_BASE0, dummymem_paddr);
  }		
	
  DMA_WRITE(BCR0, buflen);
  DMA_WRITE(CONTROL, 0x11085008);
  while (!(DMA_READ(STATUS) & 0x40)) ;
  DMA_WRITE(INTERRUPT, 0x0);
#endif
  
#ifdef CONFIG_TS7800_PLATFORM
  unsigned long bufadr, i=0, msireg;

  if (debug) printk("dmastream bufaddr: %x buflen:%d\n", (unsigned long)dat, buflen);
  
  if (dat) {

    while (buflen > (0xfff * 4)) {
      tssdcard_dmastream(os_arg, dat, (0xfff * 4));
      buflen -= (0xfff * 4);
      dat += (0xfff * 4);
    }  
  
    if ((unsigned long)dat >= VMALLOC_START) {
	  bufadr = (vmalloc_to_pfn(dat) << PAGE_SHIFT) | 
	    ((unsigned int)dat & 0xfff);
    } else {
      bufadr = virt_to_phys((char *)dat);
    }
    if (debug) printk("dmastream newbufaddr: %x\n", (unsigned long)bufadr);
    writel(bufadr, dev->dmaregs + 4);
  }
  else {
    if (dummymem_paddr == 0) {
      pte_t *pte = vmalloc_to_pte(dummymem);
      dummymem_paddr = pte_pfn(*pte) << PAGE_SHIFT;
      dummymem_paddr += (unsigned long)dummymem & 0xfff;
    }
    if (debug) printk("dmastream newbufaddr: %x\n", (unsigned long)dummymem_paddr);
    writel(dummymem_paddr, dev->dmaregs + 4);
  }		
  
  writel(TS7800_FPGASDDAT, dev->dmaregs + 8);
  
  if (dev->tssdcore.sd_state & SDDAT_RX) {
    xdma_inv_range(dat, dat + buflen);
    writel((buflen/4)|(1<<12)|(4<<16)|(2<<19), dev->dmaregs);
    if (debug) printk("dmastream read init \n");
  }
  else if (dev->tssdcore.sd_state & SDDAT_TX) {
    xdma_clean_range((char *)dat, (char *)dat + buflen);  
    writel((buflen/4)|(4<<16)|(2<<19), dev->dmaregs);
    if (debug) printk("dmastream write init \n");
  }

  
  msireg = readl(TS7800_FPGAMSIMASK); //save msi 
  writel ( (~DOORBELL_DMABIT), TS7800_CPUDOORBELL); //clear dmabit on doorbell
  writel ( (msireg | DOORBELL_DMABIT), TS7800_FPGAMSIMASK); //write/mask dmabit on msi, ready to pool  
  while ( !(readl(TS7800_CPUDOORBELL)&DOORBELL_DMABIT) ) //pooling doorbell reg until dmabit is set
  {
    //printk("dmastream waiting %d\n", readl(TS7800_CPUDOORBELL)) ;	
    i++;
	if (i<0) {
	
      DEFINE_WAIT(wait);
      prepare_to_wait(&dev->qdma, &wait, TASK_INTERRUPTIBLE);
	  //printk("sleeping dma\n");	  	  
      request_irq(dev->irq, int_handler, SA_INTERRUPT|SA_SHIRQ, dev->devname, dev);
      if (!(readl(TS7800_CPUDOORBELL)&DOORBELL_DMABIT))
        schedule();
      finish_wait(&dev->qdma, &wait);
      free_irq(dev->irq, dev);
	  //printk("wokeup dma\n");	  
	  return 0;

	}
  }
  //printk("dmastream done %d\n", readl(TS7800_CPUDOORBELL)) ;	
  writel( (msireg & ~DOORBELL_DMABIT), TS7800_FPGAMSIMASK); //restore/unmask dmabit on msi reg    
  
  if (debug) printk("dmastream done \n");    
#endif

  return 0;
}

static int tssdcard_dmaprep(void *os_arg, unsigned char *buf, unsigned int buflen)
{
  //if (debug) printk("dmaprep buflen:%d\n", buflen);

#ifdef CONFIG_MACH_TS72XX
  //invalidate_dcache_range(buf, buf + buflen);
  //dmac_inv_range(buf, buf + buflen);
  //dmac_clean_range(buf, buf + buflen);
  dmac_flush_range(buf, buf + buflen);    
#endif
  
#ifdef CONFIG_TS7800_PLATFORM
  xdma_inv_range(buf, buf + buflen);    
#endif

  return 0;
}

static void tssdcard_delay(void *arg, unsigned int us)
{
  udelay(us);
}



/*
 * Handle an I/O request.
 */
static inline void tssdcard_transfer(struct tssdcard_dev *dev, unsigned long sector,
    unsigned long nsect, char *buffer, int rw)
{
  int ret;
  
  ///? unlock here for interrupt handling
  //spin_unlock_irq(&dev->lock);
  
  //if (debug) printk("size:%d sector:%d amount:%d rw:%d\n", dev->size, sector, nsect, write);
  
  if ( (sector + nsect) > (dev->size*512/KERNEL_SECTOR_SIZE) ) {
    printk (KERN_NOTICE "Beyond-end write (%ld %ld)\n", sector, nsect);
    return;
  }
  
  del_timer(&dev->timer);
  switch (rw) {
  case WRITE:
    ret = sdwrite(&dev->tssdcore, sector, buffer, nsect);
    if (ret && !dev->tssdcore.sd_wprot) {
      sdreset(&dev->tssdcore);
      ret = sdwrite(&dev->tssdcore, sector, buffer, nsect);
    }
    //if (ret == 0) mark_buffer_uptodate(buffer, 1);
    dev->timer.expires = jiffies + (HZ / 2);
    add_timer(&dev->timer);
    break;

  case READ:  
  case READA:  
    ret = sdread(&dev->tssdcore, sector, buffer, nsect);
    if (ret) {
      sdreset(&dev->tssdcore);
      ret = sdread(&dev->tssdcore, sector, buffer, nsect);
    }
    dev->parksect = sector + nsect - 1;
    break;
  }  
  
  ///? lock back here for interrupt handling
  //spin_lock(&dev->lock);  
}

/*
 * The simple form of the request function.
 */
static void tssdcard_request(request_queue_t *q)
{
  struct tssdcard_dev *dev = q->queuedata;
  struct request *req;
  
  atomic_inc(&dev->busy);
  if ( atomic_read(&dev->busy) > 1 )
    panic("recursive make_request!\n");
	
  if (threnable)
    wake_up(&dev->event);
	
  else {  
    while ((req = elv_next_request(q)) != NULL) {
      struct tssdcard_dev *dev = req->rq_disk->private_data;
      if (! blk_fs_request(req)) {
        printk (KERN_NOTICE "Skip non-fs request\n");
        end_request(req, 0);
        continue;
      }
	
      tssdcard_transfer(dev, req->sector, req->current_nr_sectors, req->buffer, rq_data_dir(req));
	
      end_request(req, 1);
    }
  }    
  
  atomic_dec(&dev->busy);

  return;  
}


/*
 * Smarter request function that "handles clustering".
 */
static inline void tssdcard_handle_fullreq(struct tssdcard_dev *dev)
{
  struct request *req;
  struct bio *bio;
  struct bio_vec *bvec;
  int i, sectors_xferred;
  sector_t sector;

  //Transfer a set of clustered requests
  while ( (req = elv_next_request(dev->queue)) != NULL )
  {
  
    if (! blk_fs_request(req)) {
      printk (KERN_NOTICE "Skip non-fs request\n");
      end_request(req, 0);
      continue;
    }

    //Transfer a full request.
    sectors_xferred = 0;
    rq_for_each_bio(bio, req) {
	
      //Transfer a single BIO.
      //Do each segment independently.
	  sector = bio->bi_sector;
      bio_for_each_segment(bvec, bio, i) {
        char *buffer = __bio_kmap_atomic(bio, i, KM_USER0);
		
        tssdcard_transfer(dev, sector, bio_cur_sectors(bio),
            buffer, bio_data_dir(bio) );
			
        sector += bio_cur_sectors(bio);
        __bio_kunmap_atomic(bio, KM_USER0);
      }
	  
      sectors_xferred += bio->bi_size/KERNEL_SECTOR_SIZE;
    }
	
    if (! end_that_request_first(req, 1, sectors_xferred)) {
      blkdev_dequeue_request(req);
      end_that_request_last(req, 1);
    }
  }  
}

static void tssdcard_full_request(request_queue_t *q)
{
  struct tssdcard_dev *dev = q->queuedata;
  
  atomic_inc(&dev->busy);
  if ( atomic_read(&dev->busy) > 1 )
    panic("recursive make_request!\n");
	
  if (threnable)
    wake_up(&dev->event);
  else
    tssdcard_handle_fullreq(dev);
  
  atomic_dec(&dev->busy);

  return;  
}


static inline void tssdcard_add_bio(struct tssdcard_dev *dev, struct bio *bio)
{
	if (dev->biotail) {
		dev->biotail->bi_next = bio;
		dev->biotail = bio;
	} else
		dev->bio = dev->biotail = bio;
}

static inline struct bio *tssdcard_get_bio(struct tssdcard_dev *dev)
{
	struct bio *bio;

	if ((bio = dev->bio)) {
		if (bio == dev->biotail)
			dev->biotail = NULL;
		dev->bio = bio->bi_next;
		bio->bi_next = NULL;
	}

	return bio;
}

static inline void tssdcard_handle_bio(struct tssdcard_dev *dev, struct bio *bio)
{
  struct bio_vec *bvec;
  sector_t sector = bio->bi_sector;
  int i;
  
  bio_for_each_segment(bvec, bio, i) {
    char *buffer = __bio_kmap_atomic(bio, i, KM_USER0);
	
    tssdcard_transfer(dev, sector, bio_cur_sectors(bio),
      buffer, bio_data_dir(bio) );
		
    sector += bio_cur_sectors(bio);
    __bio_kunmap_atomic(bio, KM_USER0);
  }
  bio_endio(bio, bio->bi_size, 0);

  return;  
}

/*
 * The direct make request version.
 */
static int tssdcard_make_request(request_queue_t *q, struct bio *bio)
{
  struct tssdcard_dev *dev = q->queuedata;

  atomic_inc(&dev->busy);
  if ( atomic_read(&dev->busy) > 1 )
    panic("recursive make_request!\n");
	
  if (threnable) {
    tssdcard_add_bio(dev, bio);	
    wake_up(&dev->event);
  }
  else
    tssdcard_handle_bio(dev, bio);
  
  atomic_dec(&dev->busy);

  return 0;
}

/*
 * Thread code.
 */
static int tssdcard_thread(void *data)
{
  struct tssdcard_dev *dev = data;
  struct bio *bio;

  //current->flags |= PF_NOFREEZE;
  //set_user_nice(current, -20);
  set_user_nice(current, 19);

  while (!kthread_should_stop() || dev->bio) {

    wait_event_interruptible(dev->event, dev->bio || kthread_should_stop());
    if (!dev->bio)
      continue;

    spin_lock(&dev->lock);
    bio = tssdcard_get_bio(dev);
    spin_unlock(&dev->lock);

    BUG_ON(!bio);
    tssdcard_handle_bio(dev, bio);	  
  }

  return 0;
}

/*
 * Thread code.
 */
static int tssdcard_fullreq_thread(void *data)
{
  struct tssdcard_dev *dev = data;
  struct bio *bio;

  //current->flags |= PF_NOFREEZE;
  //set_user_nice(current, -20);
  set_user_nice(current, 19);

  while (!kthread_should_stop() ) {
  
    wait_event_interruptible(dev->event, elv_next_request(dev->queue)!=NULL || kthread_should_stop());
    if (!dev->queue)
      continue;
	
	///? if I include the locks with irq (spin_lock_irq), than it looses time accuracy
    spin_lock(&dev->lock);
    tssdcard_handle_fullreq(dev);
    spin_unlock(&dev->lock);
  }

  return 0;
}

/*
 * Open and close.
 */
static int tssdcard_open(struct inode *inode, struct file *filp)
{
  struct tssdcard_dev *dev = inode->i_bdev->bd_disk->private_data;
  int ret = 0;

  //if (debug) printk("open\n");
  
  filp->private_data = dev;
  
  if (!atomic_read(&dev->users))
    check_disk_change(inode->i_bdev);

  atomic_inc(&dev->users);
  
  if (threnable && dev->thread == NULL && atomic_read(&dev->users)) {
    if (reqmode == RM_FULL || reqmode == RM_SIMPLE)
      dev->thread = kthread_create(tssdcard_fullreq_thread, dev, dev->devname);
	else
      dev->thread = kthread_create(tssdcard_thread, dev, dev->devname);
	  
    if (IS_ERR(dev->thread))
      dev->thread = NULL;
	else
      wake_up_process(dev->thread);
  }  
  
  return 0;
}

static int tssdcard_release(struct inode *inode, struct file *filp)
{
  struct tssdcard_dev *dev = inode->i_bdev->bd_disk->private_data;

  //if (debug) printk("release\n");
  
  atomic_dec(&dev->users);
  if (atomic_read(&dev->users) == 0) {  
    if (dev->thread != NULL) {
      kthread_stop(dev->thread);
      dev->thread = NULL;
	}
  }

  return 0;
}

int tssdcard_media_changed(struct gendisk *gd)
{
  struct tssdcard_dev *dev = gd->private_data;
  char buf[512];
  return sdread(&dev->tssdcore, 1, buf, 1);
}

int tssdcard_revalidate(struct gendisk *gd)
{
  struct tssdcard_dev *dev = gd->private_data;

  dev->size = sdreset(&dev->tssdcore);
  if (dev->size)
    set_capacity(dev->gd, dev->size * 512 / KERNEL_SECTOR_SIZE);	

  return 0;
}


static void tssdcard_sdcommit(unsigned long ldev)
{
  struct tssdcard_dev *dev = (struct tssdcard_dev *) ldev;
  char buf[512];
  sdread(&dev->tssdcore, dev->parksect, buf, 1);
}

static int tssdcard_shutdownhook(struct notifier_block *NotifierBlock,
    unsigned long Event, void *Buffer)
{
  int i;
  ///? del_timer(&devices->timer);
  if (!(Event == SYS_RESTART || Event == SYS_HALT || Event == SYS_POWER_OFF))
    return NOTIFY_DONE;

  for (i = 0; i< ndevices ; i++) {
    tssdcard_sdcommit((unsigned long)(devices+i));
  }

  return NOTIFY_OK;
}

static struct notifier_block shutdownhook_notifier = { tssdcard_shutdownhook, NULL, 0 };


/*
 * The ioctl() implementation
 */
int tssdcard_ioctl (struct inode *inode, struct file *filp,
    unsigned int cmd, unsigned long arg)
{
  struct hd_geometry geo;
  struct tssdcard_dev *dev = filp->private_data;

  switch(cmd) {
    case HDIO_GETGEO:
	  /*
       * Get geometry: since we are a virtual device, we have to make
       * up something plausible.  So we claim 16 sectors, four heads,
       * and calculate the corresponding number of cylinders.  We set the
       * start of data at sector four.
       */
	  geo.cylinders = dev->size / (16 * 32);
      geo.heads = 16;
      geo.sectors = 32;
      geo.start = 0;
      if (copy_to_user((void __user *) arg, &geo, sizeof(geo)))
        return -EFAULT;
      return 0;
  }

  return -ENOTTY; /* unknown command */
}



/*
 * The device operations structure.
 */
static struct block_device_operations tssdcard_ops = {
  .owner			= THIS_MODULE,
  .open				= tssdcard_open,
  .release			= tssdcard_release,
  .media_changed	= tssdcard_media_changed,
  .revalidate_disk	= tssdcard_revalidate,
  .ioctl			= tssdcard_ioctl
};


/*
 * Set up our internal device.
 */
static void setup_device(struct tssdcard_dev *dev, int which)
{
  unsigned int scratchreg0; 
  unsigned long conreg;

  printk("%s: Technologic Systems SD card controller, address 0x%x\n", name, io);

  memset (dev, 0, sizeof (struct tssdcard_dev));

#ifdef CONFIG_MACH_TS72XX 
  //IO Remapping
  dev->tssdcore.sd_regstart = (unsigned long)__ioremap(io, SD_DEFIOSIZE, 0);
  //dev->tssdcore.sd_lun = which;

  //verify previous init
  conreg = (unsigned long)__ioremap(SYSCON_BASE, 0xFF, 0);
  scratchreg0 = inl(conreg|SYSCON_SCRREG0); 
  if (scratchreg0) {
    dev->tssdcore.sdboot_token = scratchreg0;
    outl(0, conreg|SYSCON_SCRREG0);
  }  
#endif
  
#ifdef CONFIG_TS7800_PLATFORM
  //IO Remapping
  dev->tssdcore.sd_regstart = (unsigned long)ioremap(io, SD_DEFIOSIZE);
  dev->tssdcore.sd_lun = which;

  //verify previous init
  conreg = (unsigned long)0;
  scratchreg0 = 0; 
  if (scratchreg0) {
    dev->tssdcore.sdboot_token = scratchreg0;
    outl(0, 0);
  }  

  dev->tssdcore.os_irqwait = NULL;  
#endif
  
  //dev and tssdcore struct initialization
  dev->tssdcore.os_arg = dev;
  dev->tssdcore.os_delay = tssdcard_delay;
  dev->tssdcore.os_dmastream = NULL;
  dev->tssdcore.os_dmaprep = NULL;
  dev->tssdcore.sd_writeparking = 1;
  dev->irq = 0;
  dev->dmach = 0;
  dev->dmaregs = 0;
  dev->parksect = 0;

  atomic_set(&dev->users,0);
  atomic_set(&dev->busy,0);

  //SD Card size and Reset
  dev->size = sdreset(&dev->tssdcore);
  if (dev->size == 0) {
    printk("%s: no card found.\n", name);
    //kfree(dev);
	//dev = NULL;
    return;
  }
  
  dev->gd = alloc_disk(TSSDCARD_MINORS);
  if (! dev->gd) {
    printk (KERN_NOTICE "alloc_disk failure\n");
    return;
  }  
  dev->devname = kmalloc(32, GFP_KERNEL);
  strcpy(dev->devname,name);
  snprintf (dev->gd->disk_name, 32, strcat(dev->devname,"%c"), which+'a');
  strcpy(dev->devname,dev->gd->disk_name);
  printk("%s: card /dev/%s has %d sectors (LUN %d)\n", name, dev->devname, dev->size, which); 
  
  //DMA initialization
#ifdef CONFIG_MACH_TS72XX 
  if (dmaenable) {
    dev->dmach = DMA_CHANNEL;
	
    if (dmach) 
      dev->dmaregs = (unsigned long) __ioremap(DMA_M2M_1_BASE, DMA_M2M_1_SIZE, 0);
    else
      dev->dmaregs = (unsigned long) __ioremap(DMA_M2M_0_BASE, DMA_M2M_0_SIZE, 0);
	  
    dev->irq = IRQ_EP93XX_DMAM2M1;
    //request_irq(dev->irq, int_handler, SA_SHIRQ, name, dev);
	
    dev->tssdcore.os_dmastream = tssdcard_dmastream;
    dev->tssdcore.os_dmaprep = tssdcard_dmaprep;		

    // Enable M2M DMA channel 1 power
    outl(0xaa, conreg|SYSCON_SWLOCK);
    if (dmach)
      outl(inl(conreg|SYSCON_PWRCNT)|SYSCON_PWRCNT_DMA_M2MCH1, conreg|SYSCON_PWRCNT);
    else
      outl(inl(conreg|SYSCON_PWRCNT)|SYSCON_PWRCNT_DMA_M2MCH0, conreg|SYSCON_PWRCNT);

    // Enable M2M DMA channel 1 DRQ pins
    outl(0xaa, conreg|SYSCON_SWLOCK);
    if (dmach)
      outl(inl(conreg|SYSCON_DEVCFG)|SYSCON_DEVCFG_D1onG, conreg|SYSCON_DEVCFG); 
    else
      outl(inl(conreg|SYSCON_DEVCFG)|SYSCON_DEVCFG_D0onG, conreg|SYSCON_DEVCFG); 
  }
  
  __iounmap((unsigned long *) conreg);
#endif
  
#ifdef CONFIG_TS7800_PLATFORM
  if (dmaenable) {
    dev->dmaregs = (unsigned long)ioremap(TS7800_DMAADDR, 0xFF);
	  
    dev->irq = TS7800_IRQ_DMA;
    dev->irq_wait = TS7800_IRQ_WAIT;
	
    init_waitqueue_head(&dev->qdma);
    init_waitqueue_head(&dev->qwait);
	
    dev->tssdcore.os_dmastream = tssdcard_dmastream;
    dev->tssdcore.os_dmaprep = tssdcard_dmaprep;		
    dev->tssdcore.os_irqwait = tssdcard_irqwait;
  }
#endif

  //lock initialization
  spin_lock_init(&dev->lock);

  //The timer which "invalidates" the device.
  init_timer(&dev->timer);
  dev->timer.data = (unsigned long) dev;
  dev->timer.function = tssdcard_sdcommit;
  register_reboot_notifier(&shutdownhook_notifier);

  dev->thread = NULL;
  if (threnable) {  
    init_waitqueue_head(&dev->event);
	
/*  
    dev->thread = kthread_create(tssdcard_thread, dev, name);
    if (IS_ERR(dev->thread))
      dev->thread = NULL;
	else
      wake_up_process(dev->thread);
*/
  }
  
  dev->bio = dev->biotail = NULL;
  
  //The I/O queue, depending on whether we are using our own make_request function or not.
  switch (reqmode) {
    case RM_NOQUEUE:
      dev->queue = blk_alloc_queue(GFP_KERNEL);
      if (dev->queue == NULL) return;
      blk_queue_make_request(dev->queue, tssdcard_make_request);
      break;

    case RM_SIMPLE:
      dev->queue = blk_init_queue(tssdcard_request, &dev->lock);
      if (dev->queue == NULL) return;
      break;

    case RM_FULL:
      dev->queue = blk_init_queue(tssdcard_full_request, &dev->lock);
      if (dev->queue == NULL) return;
      break;

    default:
      printk(KERN_NOTICE "Bad request mode %d, using simple\n", reqmode);
  }

  blk_queue_hardsect_size(dev->queue, 512);
  dev->queue->queuedata = dev;
  
  //And the gendisk structure.
  dev->gd->major = tssdcard_major;
  dev->gd->first_minor = which*TSSDCARD_MINORS;
  dev->gd->flags = GENHD_FL_REMOVABLE;
  dev->gd->fops = &tssdcard_ops;
  dev->gd->queue = dev->queue;
  dev->gd->private_data = dev;

  set_capacity(dev->gd, dev->size * 512 / KERNEL_SECTOR_SIZE);
  add_disk(dev->gd);
  
  //udelay(1000); 
  
  return;
}



static int __init tssdcard_init(void)
{
  int i;
  
  tssdcard_major = register_blkdev(tssdcard_major, name);
  if (tssdcard_major <= 0) {
    printk(KERN_WARNING "tssdcard: unable to get major number\n");
    return -EBUSY;
  }
  
  devices = kmalloc(ndevices*sizeof (struct tssdcard_dev), GFP_KERNEL);
  if (devices == NULL) 
    goto out_unregister;
	
  for (i = 0; i<ndevices; i++) 
    setup_device(devices+i, i);
  
  return 0;

out_unregister:
  unregister_blkdev(tssdcard_major, name);
  return -ENOMEM;
}

static void tssdcard_exit(void)
{
  struct tssdcard_dev *dev;
  int i;

  for (i = 0; i < ndevices; i++) {
	dev = devices + i;
	
	if (dev == NULL) continue;
	
    del_timer_sync(&dev->timer);
	
    if (dev->gd) {
      del_gendisk(dev->gd);
      put_disk(dev->gd);
    }
	
    if (dev->queue) {
      if (reqmode == RM_NOQUEUE)
        blk_put_queue(dev->queue);
      else
        blk_cleanup_queue(dev->queue);
    }
	
    if (dev->thread != NULL) {
    	kthread_stop(dev->thread);
	  dev->thread = NULL;
    }
   
#ifdef CONFIG_MACH_TS72XX 
    __iounmap((unsigned long *) dev->tssdcore.sd_regstart);
    __iounmap((unsigned long *) dev->dmaregs);
#endif
  
#ifdef CONFIG_TS7800_PLATFORM
    if (dmaenable) {
      free_irq(dev->irq, dev);	
      free_irq(dev->irq_wait, dev);
    }
	
    iounmap((unsigned long *) dev->tssdcore.sd_regstart);
    iounmap((unsigned long *) dev->dmaregs);
#endif

  }
	
  unregister_blkdev(tssdcard_major, name);
  kfree(devices);
}
  
module_init(tssdcard_init);
module_exit(tssdcard_exit);