usbio-lkm/led/usbio-led.c

#include <linux/kernel.h>
#include <linux/platform_device.h>
#include <linux/leds.h>
#include <linux/module.h>
#include <linux/init.h>
#include <linux/usb.h>
#include <linux/slab.h>

#define USBIO_VENDOR_ID  0x04d8
#define USBIO_PRODUCT_ID 0x003f

enum {
  ANSELD = 0x5e,
  TRISD = 0x95,
  CCPTMRS = 0x59,
  PR2 = 0xbb,
  CCP1CON = 0xbd,
  CCPR1L = 0xbe,
  T2CON = 0xba,
  PSTR1CON = 0xb9
};

struct usbio_led {
  struct usb_device *udev;
  struct led_classdev ldev;
  struct mutex lock;
};

int usbio_set_register(struct usb_device *usb_dev, u8 reg, u8 value)
{
  u8 data[14];
  int actual_len = 14;
  memset(data, 0, 14);
  data[10] = reg;
  data[11] = value;
  data[0] = 0x99;
  return usb_interrupt_msg(usb_dev,
			   usb_sndintpipe(usb_dev, 1),
			   data, sizeof(data), &actual_len, USB_CTRL_SET_TIMEOUT);
}

int usbio_set_register_bit(struct usb_device *usb_dev, u8 reg, u8 reg_bit, u8 value)
{
  u8 data[14];
  int actual_len = 14;
  memset(data, 0, 14);
  data[10] = reg;
  data[11] = reg_bit;
  data[12] = value;
  data[0] = 0x9b;
  return usb_interrupt_msg(usb_dev,
			   usb_sndintpipe(usb_dev, 1),
			   data, sizeof(data), &actual_len, USB_CTRL_SET_TIMEOUT);
}


static int usbio_pwm_init(struct usb_device *usb_dev)
{
  int i, error = 0;
  static const u8 register_bit_to_set[3][3] = {
    { ANSELD, 5, 0 },  
    { TRISD, 5, 1 },
    { PSTR1CON, 1, 1 }  
  };
  static const u8 register_to_set[5][2] = {
    { CCPTMRS, 0x00 },        /* select timer resource */
    { PR2, 199 },             /* load width PWM period value */
    { CCP1CON, 0b00001100 },  /* enable pwm mode bit(3-0), bit(5-4) LSB PWM duty cycle */
    { CCPR1L, 0x00 },
    { T2CON, 0b00000110 }
  };
  for(i = 0; i < 3; i++) {
    error = usbio_set_register_bit(usb_dev,
				   register_bit_to_set[i][0],
				   register_bit_to_set[i][1],
				   register_bit_to_set[i][2]);
    if(error < 0)
      return error;
  }
  for(i = 0; i < 5; i++) {
    error = usbio_set_register(usb_dev,
			       register_to_set[i][0],
			       register_to_set[i][1]);
    if(error < 0)
      return error;    
  }
  return 0;
}

static void usbio_led_set(struct led_classdev *ldev, enum led_brightness brightness) {
  struct usbio_led *led = container_of(ldev, struct usbio_led, ldev);
  mutex_lock(&led->lock);
  usbio_set_register(led->udev, CCPR1L, brightness);
  mutex_unlock(&led->lock);
}

static int usbio_led_probe(struct usb_interface *interface, const struct usb_device_id *id)
{
  struct usb_device *udev = interface_to_usbdev(interface);
  struct usbio_led *led = NULL;
  int retval = -ENOMEM;
  led = kzalloc(sizeof(struct usbio_led), GFP_KERNEL);
  if(led == NULL) {
    dev_err(&interface->dev, "Out of memory");
    goto error;
  }
  led->udev = usb_get_dev(udev);
  usb_set_intfdata(interface, led);
  dev_info(&interface->dev, "USBIO LED attached\n");
  mutex_init(&led->lock);

  mutex_lock(&led->lock);
  usbio_pwm_init(led->udev);
  mutex_unlock(&led->lock);

  led->ldev.name = "usbio_led";
  led->ldev.max_brightness = 255;
  led->ldev.brightness = LED_OFF;
  led->ldev.brightness_set = usbio_led_set;
  
  retval = led_classdev_register(&led->udev->dev, &led->ldev);

  if(retval < 0) {
    printk("whoops");
    goto error;
  }
  
  return 0;

 error:
  printk("errors");
  kfree(led);
  return retval;
}

static void usbio_led_disconnect(struct usb_interface *interface)
{
  struct usbio_led *led;
  led = usb_get_intfdata(interface);
  usb_set_intfdata(interface, NULL);
  usb_put_dev(led->udev);
  led_classdev_unregister(&led->ldev);
  kfree(led);
  dev_info(&interface->dev, "USBIO disconnected");
}

static struct usb_device_id usbio_id_table[] = {
  { USB_DEVICE(USBIO_VENDOR_ID, USBIO_PRODUCT_ID) },
  {}
};

MODULE_DEVICE_TABLE(usb, usbio_id_table);

static struct usb_driver usbio_led_driver = {
  .name = "usbio_led",
  .probe = usbio_led_probe,
  .disconnect = usbio_led_disconnect,
  .id_table = usbio_id_table
};

module_usb_driver(usbio_led_driver);

MODULE_LICENSE("GPL");
MODULE_AUTHOR("encrypt <encrypt@labr.xyz>");
MODULE_DESCRIPTION("USBIO led");