Colony » Colony Scout

#include <linux/init.h>

#include <linux/module.h>

#include <linux/fs.h>

#include <linux/cdev.h>

#include <linux/slab.h>

#include <linux/device.h>

#include <linux/semaphore.h>

#include <linux/interrupt.h>

#include <linux/gpio.h>

#include <asm/uaccess.h>

MODULE_AUTHOR("Tom Mullins");

MODULE_LICENSE("GPL");

#define BUF_LEN 20

#define N_ENCODERS 4

const int enc_pins[N_ENCODERS][2] = {{78, 79}, {80, 81}, {82, 83}, {84, 85}};

struct class *enc_class = NULL;

dev_t first_dev;

struct enc_t

{

  char buf[BUF_LEN+1];

  struct cdev cdev;

  struct semaphore sem;

  struct device *device;

  int irq[2];

  int status;

  atomic_t value;

  dev_t dev;

}

encs[N_ENCODERS];

static int enc_open(struct inode *inode, struct file *file);

static ssize_t enc_read(struct file *file, char __user *udata, size_t count,

    loff_t *offset);

static ssize_t enc_write(struct file *file, const char __user *udata,

    size_t count, loff_t *offset);

static void enc_free(struct enc_t *enc);

static int enc_init(dev_t dev, struct enc_t *enc);

struct file_operations fops =

{

  .owner = THIS_MODULE,

  .open = enc_open,

  .read = enc_read,

  .write = enc_write

};

static int enc_open(struct inode *inode, struct file *file)

{

  int i = iminor(inode);

  if (i < 0 || i >= N_ENCODERS)

  {

    return -ENXIO;

  }

  file->private_data = &encs[i];

  return 0;

}

static ssize_t enc_read(struct file *file, char __user *udata, size_t count,

    loff_t *offset)

{

  int len;

  struct enc_t *enc = file->private_data;

  if (down_interruptible(&enc->sem))

  {

    return -ERESTARTSYS;

  }

  len = snprintf(enc->buf, BUF_LEN+1, "%d\n", atomic_read(&enc->value));

  if (*offset > len)

  {

    return 0;

  }

  len -= *offset;

  if (len > count)

  {

    len = count;

  }

  copy_to_user(udata, enc->buf + *offset, len);

  *offset += len;

  up(&enc->sem);

  return len;

}

static ssize_t enc_write(struct file *file, const char __user *udata,

    size_t count, loff_t *offset)

{

  char *end;

  int new_value;

  struct enc_t *enc = file->private_data;

  if (down_interruptible(&enc->sem))

  {

    return -ERESTARTSYS;

  }

  if (count > BUF_LEN)

  {

    count = BUF_LEN;

  }

  copy_from_user(enc->buf, udata, count);

  enc->buf[count] = '\0';

  new_value = simple_strtoi(enc->buf, &end, 10);

  atomic_set(&enc->value, new_value);

  *offset += count;

  up(&enc->sem);

  return count;

}

irqreturn_t intA(int irq, void *enc, struct pt_regs *regs) {

  int this = gpio_get_value(enc->pins[0]);

  int other = gpio_get_value(enc->pins[1]);

  if (this == other) {

    atomic_inc(&enc->value);

  } else {

    atomic_dec(&enc->value);

  }

  return IRQ_HANDLED;

}

irqreturn_t intB(int irq, void *enc, struct pt_regs *regs) {

  int this = gpio_get_value(enc->pins[1]);

  int other = gpio_get_value(enc->pins[0]);

  if (this == other) {

    atomic_dec(&enc->value);

  } else {

    atomic_inc(&enc->value);

  }

  return IRQ_HANDLED;

}

static int enc_init(dev_t dev, struct enc_t *enc, int *pins)

{

  int err, i;

  // initialize members

  cdev_init(&enc->cdev, &fops);

  enc->cdev.owner = THIS_MODULE;

  enc->dev = dev;

  enc->status = 0;

  sema_init(&enc->sem, 1);

  // make cdev file operations accessible

  err = cdev_add(&enc->cdev, enc->dev, N_ENCODERS);

  if (err)

  {

    enc_free(enc);

    return err;

  }

  enc->status = 1;

  // register gpio and interrupts

  for (i = 0; i < 2; i++) {

    enc->pins[i] = pins[i];

    enc->irq[i] = gpio_to_irq(pins[i]);

    if (enc->irq[i] < 0) {

      printk("Error %d requesting irq number for gpio %d\n", enc->irq[i],

          enc->pins[i]);

      return 1;

    }

    err = gpio_direction_input(enc->pins[i]);

    if (err < 0) {

      printk("Error %d setting gpio %d to input\n", err, enc->pins[i]);

      return 1;

    }

    err = request_irq(enc->irq[i], i? intB:intA, SA_INTERRUPT, "encoder", enc);

    if (err < 0) {

      printk("Error %d requesting irq %d\n", err, enc->irq[i]);

      return 1;

    }

    err = irq_set_irq_type(enc->irq[i], IRQ_TYPE_EDGE_BOTH);

    if (err < 0) {

      printk("Error %d setting irq %d type\n", err, enc->irq[i]);

      return 1;

    }

    // TODO the error checking here does not properly clean up after itself

    // TODO perhaps we should use gpio_request? probably not necessary...

  }

  enc->status = 2;

  return 0;

}

int enc_create_dev(struct enc_t *enc) {

  // make file in /dev

  enc->device = device_create(enc_class, NULL, enc->dev, NULL, "enc%d",

      MINOR(enc->dev));

  if (IS_ERR(enc->device))

  {

    err = PTR_ERR(enc->device);

    enc->device = NULL;

    enc_free(enc);

    return err;

  }

  enc->status = 3;

  return 0;

}

static void enc_free(struct enc_t *enc)

{

  switch (enc->status)

  {

    case 3:

      device_destroy(enc_class, enc->dev);

      enc->device = NULL;

    case 2:

      free_irq(enc->irq[0], enc);

      free_irq(enc->irq[1], enc);

    case 1:

      cdev_del(&enc->cdev);

  }

  enc->status = 0;

}

static void enc_exit_module(void)

{

  int i;

  for (i = 0; i < N_ENCODERS; i++)

  {

    enc_free(&encs[i]);

  }

  if (enc_class) {

    class_destroy(enc_class);

    enc_class = NULL;

  }

  unregister_chrdev_region(first_dev, N_ENCODERS);

}

module_exit(enc_exit_module);

static int __init enc_init_module(void)

{

  int i, err;

  // allocate character devices and get major number

  err = alloc_chrdev_region(&first_dev, 0, N_ENCODERS, "encoder");

  if (err)

  {

    printk(KERN_ALERT "Error %d in alloc_chrdev_region\n", err);

    return 1;

  }

  // initialize enc structures

  for (i = 0; i < N_ENCODERS; i++) {

    err = enc_init(first_dev + i, &encs[i], enc_pins[i]);

    if (err)

    {

      printk(KERN_WARNING "Error %d initializing encoder %d\n", err, i);

      enc_exit_module();

      return 1;

    }

  }

  // register our device class for files in /dev

  enc_class = class_create(THIS_MODULE, "encoder");

  if (IS_ERR(enc_class))

  {

    err = PTR_ERR(enc_class);

    enc_class = NULL;

    printk(KERN_WARNING "Error %d creating device class\n", err);

  }

  else

  {

    // create each device

    for (i = 0; i < N_ENCODERS; i++) {

      err = enc_create_dev(&encs[i]);

      if (err)

      {

        printk(KERN_WARNING "Error %d creating enc%d\n", err, i);

        enc_exit_module();

        return 1;

      }

    }

  }

  return 0;

}

module_init(enc_init_module);