diff options
| -rw-r--r-- | drivers/hid/hid-lg4ff.c | 158 |
1 files changed, 157 insertions, 1 deletions
diff --git a/drivers/hid/hid-lg4ff.c b/drivers/hid/hid-lg4ff.c index 32c173fcb7f..1e942a01403 100644 --- a/drivers/hid/hid-lg4ff.c +++ b/drivers/hid/hid-lg4ff.c @@ -55,7 +55,10 @@ struct lg4ff_device_entry { __u16 range; __u16 min_range; __u16 max_range; - __u8 leds; +#ifdef CONFIG_LEDS_CLASS + __u8 led_state; + struct led_classdev *led[5]; +#endif struct list_head list; void (*set_range)(struct hid_device *hid, u16 range); }; @@ -335,6 +338,88 @@ static ssize_t lg4ff_range_store(struct device *dev, struct device_attribute *at return count; } +#ifdef CONFIG_LEDS_CLASS +static void lg4ff_set_leds(struct hid_device *hid, __u8 leds) +{ + struct list_head *report_list = &hid->report_enum[HID_OUTPUT_REPORT].report_list; + struct hid_report *report = list_entry(report_list->next, struct hid_report, list); + + report->field[0]->value[0] = 0xf8; + report->field[0]->value[1] = 0x12; + report->field[0]->value[2] = leds; + report->field[0]->value[3] = 0x00; + report->field[0]->value[4] = 0x00; + report->field[0]->value[5] = 0x00; + report->field[0]->value[6] = 0x00; + usbhid_submit_report(hid, report, USB_DIR_OUT); +} + +static void lg4ff_led_set_brightness(struct led_classdev *led_cdev, + enum led_brightness value) +{ + struct device *dev = led_cdev->dev->parent; + struct hid_device *hid = container_of(dev, struct hid_device, dev); + struct lg_drv_data *drv_data = (struct lg_drv_data *)hid_get_drvdata(hid); + struct lg4ff_device_entry *entry; + int i, state = 0; + + if (!drv_data) { + hid_err(hid, "Device data not found."); + return; + } + + entry = (struct lg4ff_device_entry *)drv_data->device_props; + + if (!entry) { + hid_err(hid, "Device properties not found."); + return; + } + + for (i = 0; i < 5; i++) { + if (led_cdev != entry->led[i]) + continue; + state = (entry->led_state >> i) & 1; + if (value == LED_OFF && state) { + entry->led_state &= ~(1 << i); + lg4ff_set_leds(hid, entry->led_state); + } else if (value != LED_OFF && !state) { + entry->led_state |= 1 << i; + lg4ff_set_leds(hid, entry->led_state); + } + break; + } +} + +static enum led_brightness lg4ff_led_get_brightness(struct led_classdev *led_cdev) +{ + struct device *dev = led_cdev->dev->parent; + struct hid_device *hid = container_of(dev, struct hid_device, dev); + struct lg_drv_data *drv_data = (struct lg_drv_data *)hid_get_drvdata(hid); + struct lg4ff_device_entry *entry; + int i, value = 0; + + if (!drv_data) { + hid_err(hid, "Device data not found."); + return LED_OFF; + } + + entry = (struct lg4ff_device_entry *)drv_data->device_props; + + if (!entry) { + hid_err(hid, "Device properties not found."); + return LED_OFF; + } + + for (i = 0; i < 5; i++) + if (led_cdev == entry->led[i]) { + value = (entry->led_state >> i) & 1; + break; + } + + return value ? LED_FULL : LED_OFF; +} +#endif + int lg4ff_init(struct hid_device *hid) { struct hid_input *hidinput = list_entry(hid->inputs.next, struct hid_input, list); @@ -453,6 +538,58 @@ int lg4ff_init(struct hid_device *hid) if (entry->set_range != NULL) entry->set_range(hid, entry->range); +#ifdef CONFIG_LEDS_CLASS + /* register led subsystem - G27 only */ + entry->led_state = 0; + for (j = 0; j < 5; j++) + entry->led[j] = NULL; + + if (lg4ff_devices[i].product_id == USB_DEVICE_ID_LOGITECH_G27_WHEEL) { + struct led_classdev *led; + size_t name_sz; + char *name; + + lg4ff_set_leds(hid, 0); + + name_sz = strlen(dev_name(&hid->dev)) + 8; + + for (j = 0; j < 5; j++) { + led = kzalloc(sizeof(struct led_classdev)+name_sz, GFP_KERNEL); + if (!led) { + hid_err(hid, "can't allocate memory for LED %d\n", j); + goto err; + } + + name = (void *)(&led[1]); + snprintf(name, name_sz, "%s::RPM%d", dev_name(&hid->dev), j+1); + led->name = name; + led->brightness = 0; + led->max_brightness = 1; + led->brightness_get = lg4ff_led_get_brightness; + led->brightness_set = lg4ff_led_set_brightness; + + entry->led[j] = led; + error = led_classdev_register(&hid->dev, led); + + if (error) { + hid_err(hid, "failed to register LED %d. Aborting.\n", j); +err: + /* Deregister LEDs (if any) */ + for (j = 0; j < 5; j++) { + led = entry->led[j]; + entry->led[j] = NULL; + if (!led) + continue; + led_classdev_unregister(led); + kfree(led); + } + goto out; /* Let the driver continue without LEDs */ + } + } + } +#endif + +out: hid_info(hid, "Force feedback for Logitech Speed Force Wireless by Simon Wood <simon@mungewell.org>\n"); return 0; } @@ -474,6 +611,25 @@ int lg4ff_deinit(struct hid_device *hid) hid_err(hid, "Error while deinitializing device, no device properties data.\n"); return -1; } + +#ifdef CONFIG_LEDS_CLASS + { + int j; + struct led_classdev *led; + + /* Deregister LEDs (if any) */ + for (j = 0; j < 5; j++) { + + led = entry->led[j]; + entry->led[j] = NULL; + if (!led) + continue; + led_classdev_unregister(led); + kfree(led); + } + } +#endif + /* Deallocate memory */ kfree(entry); |