#include <linux/module.h>
#include <linux/init.h>
#include <linux/slab.h>
#include <linux/fs.h>
#include <linux/cdev.h>
#include <linux/device.h>
#include <linux/io.h>
#include <linux/of.h>
#include <asm/uaccess.h>

MODULE_DESCRIPTION("Raspberry Pi GPIO FDD Driver");
MODULE_VERSION("20171012");
MODULE_AUTHOR("AKIKAWA, Hisashi <icb49250@nifty.ne.jp>");
MODULE_LICENSE("Dual BSD/GPL");

#define DEV_NAME "gpiofd"
#define DATA_SIZE 500000	/* 1/300rpm/2us * 4bytes + more */
#define MAP_SIZE (41*4)

#define PERI_BASE1 0x20000000
#define PERI_BASE2 0x3f000000
#define GPIO_OFFSET 0x200000
#define ST_OFFSET 0x3000
#define ARMCTRL_TIMER0_1_OFFSET 0xb400

#define GPIO_INDEX	4
#define GPIO_READ	8
#define GPIO_WRITE	23
#define GPIO_GATE	24

#define GPSET0		(0x1c / 4)
#define GPCLR0		(0x28 / 4)
#define GPLEV0		(0x34 / 4)

#define TEST(x, y)	!((x) & (1 << (y)))
#define ON(x)		TEST(gpio[GPLEV0 + (x) / 0x20], ((x) % 0x20))
#define OFF(x)		!ON(x)

#define AFTER_SEEK	18000	/* wait after seek = 18ms */
#define AFTER_GATE	1000	/* wait after gate = 1ms */

#define PRECOMP		125	/* write precompensation = 125ns */

uint32_t getperibase(void);
static int gpiofd_init(void);
static void gpiofd_exit(void);
int gpiofd_open(struct inode *inode, struct file *file);
int gpiofd_close(struct inode *inode, struct file *file);
ssize_t gpiofd_read(struct file *file, char __user *buf, size_t count, loff_t *fops);
ssize_t gpiofd_write(struct file *file, const char __user *buf, size_t count, loff_t *fops);

int getfreq(void);
inline void uwait(int time);
inline void set(int gpionum);
inline void res(int gpionum);

static dev_t gpiofd_dev;
static dev_t gpiofd_devno;
static struct cdev gpiofd_cdev;
static struct class *gpiofd_class;
static uint32_t *data;

static volatile uint32_t *gpio;
static volatile uint32_t *timer_peri;
static volatile uint32_t *timer_arm;

struct file_operations gpiofd_fops = {
  .open = gpiofd_open,
  .release = gpiofd_close,
  .read = gpiofd_read,
  .write = gpiofd_write,
};


static int gpiofd_init(void)
{
  uint32_t peri_base;

  printk(KERN_ALERT "%s: driver loaded\n", DEV_NAME);

  data = kmalloc(DATA_SIZE, GFP_KERNEL);
  if (data == NULL) {
    printk(KERN_ALERT "%s: failed to kmalloc\n", DEV_NAME);
    goto init_error;
  }
  memset(data, 0, DATA_SIZE);

  if (alloc_chrdev_region(&gpiofd_dev, 0, 1, DEV_NAME)) {
    printk(KERN_ALERT "%s: failed to alloc_chrdev_region\n", DEV_NAME);
    goto init_error;
  }

  cdev_init(&gpiofd_cdev, &gpiofd_fops);
  gpiofd_cdev.owner = THIS_MODULE;

  if (cdev_add(&gpiofd_cdev, gpiofd_dev, 1)) {
    printk(KERN_ALERT "%s: failed to cdev_add\n", DEV_NAME);
    goto init_error;
  }

  gpiofd_class = class_create(gpiofd_cdev.owner, DEV_NAME);
  if (IS_ERR(gpiofd_class)) {
    printk(KERN_ALERT "%s: failed to class_create\n", DEV_NAME);
    goto init_error;
  }

  gpiofd_devno = MKDEV(MAJOR(gpiofd_dev), MINOR(gpiofd_dev));
  if (IS_ERR(device_create(gpiofd_class, NULL, gpiofd_devno, NULL, DEV_NAME))) {
    printk(KERN_ALERT "%s: failed to device_create\n", DEV_NAME);
    goto init_error;
  }

  peri_base = getperibase();
  gpio = ioremap_nocache(peri_base + GPIO_OFFSET, MAP_SIZE);
  timer_peri = ioremap_nocache(peri_base + ST_OFFSET + 4, 8);
  timer_arm = ioremap_nocache(peri_base + ARMCTRL_TIMER0_1_OFFSET, 36);
  timer_arm[2] = 0x00000282;	  /* 32-bit, no pre-scale, timer enabled */

  return 0;

 init_error:
  gpiofd_exit();
  return -1;
}

static void gpiofd_exit(void)
{
  if (gpio) {
    iounmap(gpio);
    gpio = NULL;
  }
  if (timer_peri) {
    iounmap(timer_peri);
    timer_peri = NULL;
  }
  if (timer_arm) {
    iounmap(timer_arm);
    timer_arm = NULL;
  }

  if (data) {
    kfree(data);
    data = NULL;
  }
  if (gpiofd_class) {
    device_destroy(gpiofd_class, gpiofd_devno);
    class_destroy(gpiofd_class);
    gpiofd_class = NULL;
  }

  if (gpiofd_dev){
    unregister_chrdev_region(gpiofd_dev, 1);
    gpiofd_dev = 0;
  }
  
  printk(KERN_ALERT "%s: driver unloaded\n", DEV_NAME);
}

int gpiofd_open(struct inode *inode, struct file *file)
{
  return 0;
}

int gpiofd_close(struct inode *inode, struct file *file)
{
  return 0;
}

ssize_t gpiofd_read(struct file *file, char __user *buf, size_t count, loff_t *fops)
{
  int i;
  int freq;
  int ndata = 0;
  uint32_t start;
  uint32_t tmp;
  uint32_t high, low;

  /* wait for stepping and settling */
  uwait(AFTER_SEEK);

  /* time out */
  local_irq_disable();
  local_fiq_disable();

  start = timer_peri[0];
  while (OFF(GPIO_INDEX)) {
    if ((uint32_t)(timer_peri[0] - start) >= 4000000) goto stop_read;
  }

  /* wait for index signal edge */
  while (ON(GPIO_INDEX)) ;
  while (!TEST(tmp = gpio[GPLEV0], GPIO_INDEX)) ;
  start = timer_arm[8];

  while (TEST(tmp, GPIO_READ)) {
    tmp = gpio[GPLEV0];
  }

  /* read data */
  while (1) {
    while (!TEST(tmp, GPIO_READ)) {
      if (!TEST(tmp = gpio[GPLEV0], GPIO_INDEX)) goto after_index;
    }
    data[ndata++] = timer_arm[8];
    if (ndata >= DATA_SIZE / 4) goto stop_read;
    while (TEST(tmp = gpio[GPLEV0], GPIO_READ)) ;
  }

after_index:
  while (1) {
    while (!TEST(tmp, GPIO_READ)) {
      if (TEST(tmp = gpio[GPLEV0], GPIO_INDEX)) {
	data[ndata++] = timer_arm[8];
	goto stop_read;
      }
    }
    data[ndata++] = timer_arm[8];
    if (ndata >= DATA_SIZE / 4) goto stop_read;
    while (TEST(tmp = gpio[GPLEV0], GPIO_READ)) ;
  }

 stop_read:
  /* ARM timer frequency */
  freq = getfreq();

  local_fiq_enable();
  local_irq_enable();

  for (i = 0; i < ndata; i++) {
//    data[i] = (data[i] - start) * 1000 / freq;	/* [ns] */
    high = (data[i] - start) >> 16;
    low = (data[i] - start) & 0xffff;
    data[i] = ((high * 1000 / freq) << 16) +
      (((high * 1000 % freq) << 16) + low * 1000) / freq;
  }

  if (ndata) {
    if (copy_to_user(buf, (unsigned char *)data, ndata * 4)) {
      printk(KERN_ALERT "%s: failed to copy_to_user (size=%d)\n",
	     DEV_NAME, ndata * 4);
      return -1;
    }
  }

  return ndata * 4;
}

ssize_t gpiofd_write(struct file *file, const char __user *buf, size_t count, loff_t *fops)
{
  int i;
  int freq;
  int index_old;
  int before, after;
  uint32_t start, time;
  uint32_t high, low;

  if (count > DATA_SIZE) {
    printk(KERN_ALERT "%s: too much size\n", DEV_NAME);
    return -1;
  }

  if (copy_from_user(data, buf, count)) {
    printk(KERN_ALERT "%s: failed to copy_from_user (size=%d)\n",
	   DEV_NAME, count);
    return -1;
  }

  /* write precompensation */
  before = (data[1] - data[0] + 500) / 1000;
  for (i = 1; i < (int)count / 4 - 2; i++) {
    after = (data[i + 1] - data[i] + 500) / 1000;
    if (before != after) {
      if (min(before, after) < 3) {
	data[i] += PRECOMP * (before - after) / abs(before - after);
      } else {
	data[i] += PRECOMP * (before - after) / abs(before - after) / 4;
      }
    }
    before = after;
  }

  /* wait for stepping and settling */
  uwait(AFTER_SEEK);

  /* adjust frequency */
  local_irq_disable();
  local_fiq_disable();

  freq = getfreq();
  for (i = 0; i < count / 4; i++) {
//    data[i] = data[i] * freq / 1000;
    high = data[i] >> 16;
    low = data[i] & 0xffff;
    data [i] = ((high * freq / 1000) << 16) +
      (((high * freq % 1000) << 16) + low * freq) / 1000;
  }

  /* time out */
  start = timer_peri[0];
  while (OFF(GPIO_INDEX)) {
    if ((uint32_t)(timer_peri[0] - start) >= 4000000) goto stop_write;
  }

  /* set write gate */
  res(GPIO_WRITE);
  set(GPIO_GATE);

  /* wait for index signal edge */
  while(ON(GPIO_INDEX)) ;
  while(OFF(GPIO_INDEX)) ;
  while(ON(GPIO_INDEX)) ;
  while(OFF(GPIO_INDEX)) ;

  start = timer_arm[8];
  index_old = 1;
  i = 0;

  /* write data */
  while (1) {
    if (ON(GPIO_INDEX)) {
      if (index_old == 0) {
	break;
      }
    } else {
      index_old = 0;
    }

    if (i < (int)count / 4 - 1) {
      do {
	time = timer_arm[8];
      } while ((uint32_t)(time - start) < data[i]) ;
      set(GPIO_WRITE);
      /* pulse width = 250ns */
      while ((uint32_t)(timer_arm[8] - time) < (uint32_t)(freq / 4)) ;
      res(GPIO_WRITE);
      i++;
    }
  }

 stop_write:
  res(GPIO_GATE);
  uwait(AFTER_GATE);

  local_fiq_enable();
  local_irq_enable();

  return count;
}


module_init(gpiofd_init);
module_exit(gpiofd_exit);


/* wait 1ms and get system clock frequency [MHz] */
int getfreq()
{
  uint32_t start, time;

  start = timer_peri[0];
  while (timer_peri[0] == start) ;
  time = timer_arm[8];
  while (timer_peri[0] - start < 1001) ;

  return (uint32_t)(timer_arm[8] - time + 500) / 1000;
}

/* wait for time [us] */
inline void uwait(int time)
{
  uint32_t start;

  start = timer_peri[0];
  if (time <= 0) return;
  while ((uint32_t)(timer_peri[0] - start) < time) ;
}

/* set signal on (=low level) */
inline void set(int gpionum)
{
  gpio[GPCLR0 + gpionum / 0x20] = 1 << (gpionum % 0x20);
}

/* set signal off (=high level) */
inline void res(int gpionum)
{
  gpio[GPSET0 + gpionum / 0x20] = 1 << (gpionum % 0x20);
}

/* get peripheral base address */
uint32_t getperibase(void)
{
  uint32_t peri_base;
  uint32_t ranges[3];
  const struct device_node *node;

  peri_base = 0;
  node = of_find_node_by_name(NULL, "soc");
  if (node && !of_property_read_u32_array(node, "ranges", ranges, 3)) {
    peri_base = ranges[1];
  }
  if (peri_base == 0) {
    peri_base = PERI_BASE2;
    printk(KERN_ALERT "%s: guess Raspberry Pi 2\n", DEV_NAME);
  }
  printk(KERN_ALERT "%s: peripheral base address=0x%08x\n", DEV_NAME, peri_base);

  return peri_base;
}