/*
 Kernel driver for the Mini SSC II Serial Servo Controller

 Authors: 
   Catalin Drula, Travis Choma, Matthew Packer, Stefan Camilleri, 
        Kamil Kowalski

 For compiling:
    gcc -D__KERNEL__ -DMODULE -O2 -I/usr/include -c minisscdriver.c

 Usage:
    create devices /dev/pwm0, ..., /dev/pwm7 (corresponding to the 8 servos)
       with major number 254
    write byte Y to /dev/pwmx to move servo Y to position x
*/
 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include 
#include 

#include 

#include 
#include 

#define IRQ_PWM 3                    
#define DEFAULT_PWM_BASE 0x2f8       // (default port: /dev/ttyS1)

static int pwm_base = 0;      /* This is defined by user or init_module */
static int pwm_major = 0;    

typedef struct {
         int servo;
         unsigned char position;
        } command;

#define BSIZE 4096
static command buffer[BSIZE];
static int tail,head;

/* queue of bottom halves */
struct tq_struct pwm_tq;

static inline void serial_out(unsigned char value, int offset) {
 
   outb(value, pwm_base+offset);

}

static int pwm_open(struct inode *inode, struct file *filp) {
  
  MOD_INC_USE_COUNT;

  return 0;
} 

static ssize_t pwm_write(struct file *filp, const char *buf,
                         size_t count, loff_t *offset) {
  int retval = count;
  unsigned char position, *kbuf, *ptr;
  
  /* get the minor number, this is the number of servo to send command */
  int minor = (MINOR(filp->f_dentry->d_inode->i_rdev));

  /* read the bytes from user space */
  kbuf = (unsigned char *) kmalloc(count, GFP_KERNEL);
  if (!kbuf) {
    printk("minisscdriver: failed to allocate memory\n");
    return -ENOMEM;
  }

  copy_from_user(kbuf, buf, count);
  ptr=kbuf;

  /* the mini-scc controls 8 servos */
  if (minor<0 || minor>7) {
    printk("Invalid minor number (it has to be 0-7)\n");
    return -EINVAL;
  }

  while (count--) {
   
    position = *(ptr++);
    
      /* queue the mini ssc command, will be output by bh */ 
      if((head+1)%BSIZE == tail) {
        printk("Buffer Full, dropping command\n");
        break;
	} else {
        #ifdef DEBUG
	  printk("adding %d %d to buffer\n",minor,position);
        #endif
        buffer[head].servo = minor;
        buffer[head].position = position;
        head = (head+1)%BSIZE;
      }
    
      /* enable empty transmit register interrupt now that
         we have stuff in our buffer */
      serial_out(0x02, UART_IER);      
 
  };
  
  kfree(kbuf);
  return retval;
}

void pwm_handler(int irq, void *dev_id, struct pt_regs *regs) {
  unsigned char ival;
  
  #ifdef DEBUG
    printk("top half: interrupt received...\n");
  #endif

  ival = inb(pwm_base + UART_IIR);
  /* if its an empty transmit reg interrupt queue the bh routine */
  if(ival & UART_IIR_THRI) {
    queue_task(&pwm_tq, &tq_immediate);
    mark_bh(IMMEDIATE_BH);
  }
}

void bh_pwm(void *unused) {
  int c=0;
  
  /* fill the UART FIFO */
  #ifdef DEBUG
    printk("bottom half:....\n");
  #endif
  
  /* fill the FIFO with 5 commands (15 bytes) */
  while (tail!=head && c<5) {
    c++;
    #ifdef DEBUG
     printk("bh output: servo: %d position %d\n",
                    buffer[tail].servo,buffer[tail].position);
    #endif
     serial_out(0xFF,UART_TX); /* sync byte */
    serial_out(buffer[tail].servo,UART_TX); 
    serial_out(buffer[tail].position,UART_TX);
    tail = (tail+1)%BSIZE;
  }
}

static int pwm_release(struct inode *inode, struct file *filp){ 
  MOD_DEC_USE_COUNT;

  return 0;
}

struct file_operations pwm_fops = {
  NULL,
  NULL,
  NULL,
  pwm_write,
  NULL,
  NULL,
  NULL,
  NULL,
  pwm_open,
  NULL,
  pwm_release
};


/* Module initialization function */
int init_module(void) {
  int result;
  unsigned char cval;

  if (!pwm_base)
    pwm_base = DEFAULT_PWM_BASE;

  /* Allocating port region */
  if (request_region(pwm_base, 8, "pwm") == NULL) {
    printk("minisscdriver: failed to get I/O address 0x%x\n", pwm_base);
    return -1;
  }

  /* Registering major device number */
  result = register_chrdev(pwm_major, "pwm", &pwm_fops);
  if (result < 0) {
    printk("minisscdriver: cannot get major number\n");
    release_region(pwm_base, 8);
    return result;
  }
  if (pwm_major == 0) pwm_major = result;    /* dynamic major number alloc. */

  /* intialize our buffer */  
  head=0;
  tail=0;

  /* set up IRQ handling */
  pwm_tq.routine = bh_pwm;
  pwm_tq.data = NULL;

  result = request_irq(IRQ_PWM,pwm_handler,0,"PWM",NULL);

  if(result) { 
    printk("failed to install irq %i handler...\n",IRQ_PWM);
    return result;
  }

  cval = 0;

  cval |= UART_LCR_WLEN8;       /* enable 8 data bits mode */
  cval |= UART_LCR_DLAB;        /* enable this to set baud rate */

  serial_out(cval, UART_LCR);    /* set 8-N-1 mode on serial port */
  
  serial_out(0x00, UART_DLM);    /* set 2400 bps mode on serial port */   
  serial_out(0x30, UART_DLL);    

  cval &= ~UART_LCR_DLAB;       /* enable access to THR and IER regs */
  serial_out(cval, UART_LCR);

  serial_out(0xc7, UART_FCR);   /* enable FIFOs */

  serial_out(0x02, UART_IER);   /* enable interrupts */

  serial_out(0x08, UART_MCR);   /* enable OUT2 (for servicing interrupts)*/

  return 0;
}


void cleanup_module(void) {
  unregister_chrdev(pwm_major, "pwm");
  release_region(pwm_base, 8);
  free_irq(IRQ_PWM,NULL);
}