From: Michal Malý Date: Wed, 13 Aug 2014 08:48:33 +0000 (+0200) Subject: MAJOR CHANGES! X-Git-Url: https://gitweb.devoid-pointer.net/?a=commitdiff_plain;h=e2289f3e634ddd47cad5858d0c686be4a9917c8c;p=KLGD_FF_plugin.git MAJOR CHANGES! - Create a virtual Force Feedback device to test KLGD and FFB handling - Add klgd_ff_plugin --- diff --git a/Makefile b/Makefile index c2f44f8..7c20a8d 100644 --- a/Makefile +++ b/Makefile @@ -1,8 +1,11 @@ -# You will want to fix this line + # You will want to fix this line KBUILD_EXTRA_SYMBOLS := /home/madcat/Devel/KLGD/Module.symvers +KBUILD_CFLAGS += -g3 ifneq ($(KERNELRELEASE),) - obj-m += klgdtm.o + klgdffm-y := klgd_ff_plugin.o + klgdffm-y += klgdff.o + obj-m += klgdffm.o else KERNELDIR ?= /lib/modules/$(shell uname -r)/build diff --git a/klgd_ff_plugin.c b/klgd_ff_plugin.c new file mode 100644 index 0000000..86556bb --- /dev/null +++ b/klgd_ff_plugin.c @@ -0,0 +1,329 @@ +#include "klgd_ff_plugin.h" +#include "klgd_ff_plugin_p.h" +#include + +static bool ffpl_replace_effect(const struct ff_effect *ac_eff, const struct ff_effect *la_eff); + +static void ffpl_destroy_rq(struct ff_device *ff) +{ + struct klgd_plugin *self = ff->private; + struct klgd_plugin_private *priv = self->private; + size_t idx; + + for (idx = 0; idx < priv->effect_count; idx++) { + struct ffpl_effect *eff = &priv->effects[idx]; + + kfree(eff->active); + if (eff->active != eff->latest) + kfree(eff->latest); + } + kfree(priv->effects); + kfree(priv); + /* Prevent double free in klgd_deinit() */ + ff->private = NULL; +} + +static int ffpl_erase_rq(struct input_dev *dev, int effect_id) +{ + struct klgd_plugin *self = dev->ff->private; + struct klgd_plugin_private *priv = self->private; + struct ffpl_effect *eff = &priv->effects[effect_id]; + + klgd_lock_plugins(self->plugins_lock); + eff->change = FFPL_TO_ERASE; + klgd_unlock_plugins_sched(self->plugins_lock); + + return 0; +} + +static int ffpl_playback_rq(struct input_dev *dev, int effect_id, int value) +{ + struct klgd_plugin *self = dev->ff->private; + struct klgd_plugin_private *priv = self->private; + struct ffpl_effect *eff = &priv->effects[effect_id]; + + klgd_lock_plugins(self->plugins_lock); + + if (value) + eff->change = FFPL_TO_START; + else + eff->change = FFPL_TO_STOP; + + klgd_unlock_plugins_sched(self->plugins_lock); + + return 0; +} + +static int ffpl_upload_rq(struct input_dev *dev, struct ff_effect *effect, struct ff_effect *old) +{ + struct klgd_plugin *self = dev->ff->private; + struct klgd_plugin_private *priv = self->private; + struct ffpl_effect *eff = &priv->effects[effect->id]; + + klgd_lock_plugins(self->plugins_lock); + spin_lock_irq(&dev->event_lock); + + + /* Latest effect is not in use, free it */ + if (eff->latest != eff->active) + kfree(eff->latest); + /* Copy the new effect to the "latest" slot */ + eff->latest = kmemdup(effect, sizeof(struct ff_effect), GFP_KERNEL); + + if (eff->state != FFPL_EMPTY) { + if (ffpl_replace_effect(eff->active, eff->latest)) + eff->replace = true; + else { + eff->replace = false; + eff->change = FFPL_TO_UPDATE; + } + } else + eff->change = FFPL_TO_UPLOAD; + + spin_unlock_irq(&dev->event_lock); + klgd_unlock_plugins_sched(self->plugins_lock); + + return 0; +} + +static void ffpl_set_gain_rq(struct input_dev *dev, u16 gain) +{ + struct klgd_plugin *self = dev->ff->private; + + klgd_lock_plugins(self->plugins_lock); + + //TODO + printk(KERN_DEBUG "KLGDFF: Gain set, %u\n", gain); + // + + klgd_unlock_plugins_sched(self->plugins_lock); +} + +static void ffpl_deinit(struct klgd_plugin *self) +{ + printk(KERN_DEBUG "KLGDFF: Deinit complete\n"); +} + +static struct klgd_command_stream * ffpl_get_commands(struct klgd_plugin *self, const unsigned long now) +{ + struct klgd_plugin_private *priv = self->private; + struct klgd_command_stream *s; + size_t idx; + + s = klgd_alloc_stream(); + if (!s) + return NULL; /* TODO: Error handling */ + + for (idx = 0; idx < priv->effect_count; idx++) { + struct ffpl_effect *eff = &priv->effects[idx]; + + /* Effect has not been touched since the last update, skip it */ + if (eff->change == FFPL_DONT_TOUCH) + continue; + + /* Latest effect is of different type than currently active effect, + * remove it from the device and upload the latest one */ + if (eff->replace) { + switch (eff->state) { + case FFPL_STARTED: + klgd_append_cmd(s, priv->stop_effect(eff->active, idx)); + default: + klgd_append_cmd(s, priv->erase_effect(eff->active, idx)); + kfree(eff->active); + eff->active = NULL; + break; + } + eff->replace = false; + + /* The new effect is to be erased anyway */ + if (eff->change == FFPL_TO_ERASE) { + eff->change = FFPL_DONT_TOUCH; + continue; + } + } + + /* Figure out the state change since the last update and build + * command stream accordingly */ + switch (eff->state) { + case FFPL_EMPTY: + switch (eff->change) { + case FFPL_TO_UPLOAD: + case FFPL_TO_STOP: + klgd_append_cmd(s, priv->upload_effect(eff->latest, idx)); + eff->active = eff->latest; + eff->state = FFPL_UPLOADED; + break; + case FFPL_TO_START: + klgd_append_cmd(s, priv->upload_effect(eff->latest, idx)); + eff->active = eff->latest; + klgd_append_cmd(s, priv->start_effect(eff->active, idx)); + eff->state = FFPL_STARTED; + break; + default: + break; + } + break; + case FFPL_UPLOADED: + switch (eff->change) { + case FFPL_TO_START: + klgd_append_cmd(s, priv->start_effect(eff->active, idx)); + eff->state = FFPL_STARTED; + break; + case FFPL_TO_ERASE: + klgd_append_cmd(s, priv->erase_effect(eff->active, idx)); + kfree(eff->active); + if (eff->active != eff->latest) + kfree(eff->latest); + eff->latest = NULL; + eff->active = NULL; + eff->state = FFPL_EMPTY; + break; + case FFPL_TO_UPDATE: + klgd_append_cmd(s, priv->upload_effect(eff->latest, idx)); + eff->active = eff->latest; + break; + default: + break; + } + break; + case FFPL_STARTED: + switch (eff->change) { + case FFPL_TO_STOP: + klgd_append_cmd(s, priv->stop_effect(eff->active, idx)); + eff->state = FFPL_UPLOADED; + break; + case FFPL_TO_ERASE: + klgd_append_cmd(s, priv->stop_effect(eff->active, idx)); + klgd_append_cmd(s, priv->erase_effect(eff->active, idx)); + eff->state = FFPL_EMPTY; + kfree(eff->active); + if (eff->active != eff->latest) + kfree(eff->latest); + eff->latest = NULL; + eff->active = NULL; + eff->state = FFPL_EMPTY; + break; + case FFPL_TO_UPDATE: + klgd_append_cmd(s, priv->upload_effect(eff->latest, idx)); + eff->active = eff->latest; + default: + break; + } + break; + } + eff->change = FFPL_DONT_TOUCH; + } + + return s; +} + +static bool ffpl_get_update_time(struct klgd_plugin *self, const unsigned long now, unsigned long *t) +{ + struct klgd_plugin_private *priv = self->private; + size_t idx, events = 0; + + for (idx = 0; idx < priv->effect_count; idx++) { + struct ffpl_effect *eff = &priv->effects[idx]; + + /* Tell KLGD to attend to us as soon as possible if an effect has to change state */ + if (eff->change == FFPL_DONT_TOUCH) + continue; + *t = now; + events++; + } + + return events ? true : false; +} + +static int ffpl_init(struct klgd_plugin *self) +{ + struct klgd_plugin_private *priv = self->private; + struct input_dev *dev = priv->dev; + int ret; + + ret = input_ff_create(dev, priv->effect_count); + if (ret) + return ret; + + dev->ff->private = self; + dev->ff->erase = ffpl_erase_rq; + dev->ff->playback = ffpl_playback_rq; + dev->ff->upload = ffpl_upload_rq; + dev->ff->set_gain = ffpl_set_gain_rq; + dev->ff->destroy = ffpl_destroy_rq; + printk(KERN_NOTICE "KLGDFF: Init complete\n"); + + return 0; +} + +/* Initialize the plugin */ +int ffpl_init_plugin(struct klgd_plugin **plugin, struct input_dev *dev, const size_t effect_count, + const unsigned long supported_effects, + struct klgd_command * (*upload)(const struct ff_effect *effect, const int id), + struct klgd_command * (*start)(const struct ff_effect *effect, const int id), + struct klgd_command * (*stop)(const struct ff_effect *effect, const int id), + struct klgd_command * (*erase)(const struct ff_effect *effect, const int id)) +{ + struct klgd_plugin *self; + struct klgd_plugin_private *priv; + int ret, idx; + + self = kzalloc(sizeof(struct klgd_plugin), GFP_KERNEL); + if (!self) + return -ENOMEM; + + priv = kzalloc(sizeof(struct klgd_plugin_private), GFP_KERNEL); + if (!priv) { + ret = -ENOMEM; + goto err_out1; + } + + priv->effects = kzalloc(sizeof(struct ffpl_effect) * effect_count, GFP_KERNEL); + if (!priv->effects) { + ret = -ENOMEM; + goto err_out2; + } + + self->deinit = ffpl_deinit; + self->get_commands = ffpl_get_commands; + self->get_update_time = ffpl_get_update_time; + self->init = ffpl_init; + priv->supported_effects = supported_effects; + priv->effect_count = effect_count; + priv->dev = dev; + priv->upload_effect = upload; + priv->start_effect = start; + priv->stop_effect = stop; + priv->erase_effect = erase; + + self->private = priv; + *plugin = self; + + set_bit(FF_GAIN, dev->ffbit); + for (idx = 0; idx < (FF_EFFECT_MAX - FF_EFFECT_MIN); idx++) { + if (test_bit(idx, &priv->supported_effects)) { + printk(KERN_NOTICE "KLGDFF: Has bit %d, effect type %d\n", idx, FF_EFFECT_MIN + idx); + input_set_capability(dev, EV_FF, idx + FF_EFFECT_MIN); + } + } + return 0; + +err_out2: + kfree(self->private->effects); +err_out1: + kfree(self); + return ret; +} + +static bool ffpl_replace_effect(const struct ff_effect *ac_eff, const struct ff_effect *la_eff) +{ + if (ac_eff->type != la_eff->type) + return true; + + if (ac_eff->type == FF_PERIODIC) { + if (ac_eff->u.periodic.waveform != la_eff->u.periodic.waveform) + return true; + } + + return false; +} diff --git a/klgd_ff_plugin.h b/klgd_ff_plugin.h new file mode 100644 index 0000000..f6eaf52 --- /dev/null +++ b/klgd_ff_plugin.h @@ -0,0 +1,11 @@ +#include +#include "../KLGD/klgd.h" + +#define FFPL_EFBIT(x) BIT(x - FF_EFFECT_MIN) + +int ffpl_init_plugin(struct klgd_plugin **plugin, struct input_dev *dev, const size_t effect_count, + const unsigned long supported_effects, + struct klgd_command * (*upload)(const struct ff_effect *effect, const int id), + struct klgd_command * (*play)(const struct ff_effect *effect, const int id), + struct klgd_command * (*stop)(const struct ff_effect *effect, const int id), + struct klgd_command * (*erase)(const struct ff_effect *effect, const int id)); diff --git a/klgd_ff_plugin_p.h b/klgd_ff_plugin_p.h new file mode 100644 index 0000000..4e302b7 --- /dev/null +++ b/klgd_ff_plugin_p.h @@ -0,0 +1,36 @@ + +/* State change flags */ +enum ffpl_st_change { + FFPL_DONT_TOUCH, + FFPL_TO_UPLOAD, + FFPL_TO_START, + FFPL_TO_STOP, + FFPL_TO_ERASE, + FFPL_TO_UPDATE +}; + +/* Status flags */ +enum ffpl_state { + FFPL_EMPTY, + FFPL_UPLOADED, + FFPL_STARTED, +}; + +struct ffpl_effect { + struct ff_effect *active; + struct ff_effect *latest; + enum ffpl_st_change change; + enum ffpl_state state; + bool replace; +}; + +struct klgd_plugin_private { + struct ffpl_effect *effects; + unsigned long supported_effects; + size_t effect_count; + struct input_dev *dev; + struct klgd_command * (*upload_effect)(const struct ff_effect *effect, const int id); + struct klgd_command * (*start_effect)(const struct ff_effect *effect, const int id); + struct klgd_command * (*stop_effect)(const struct ff_effect *effect, const int id); + struct klgd_command * (*erase_effect)(const struct ff_effect *effect, const int id); +}; diff --git a/klgdff.c b/klgdff.c new file mode 100644 index 0000000..f63becb --- /dev/null +++ b/klgdff.c @@ -0,0 +1,144 @@ +#include +#include +#include +#include +#include +#include "klgd_ff_plugin.h" + +#define EFFECT_COUNT 8 + +static struct kobject *klgdff_obj; + +static struct input_dev *dev; +static struct klgd_main klgd; +static struct klgd_plugin *ff_plugin; + +static struct klgd_command * klgdff_erase(const struct ff_effect *effect, const int id) +{ + char *text = kasprintf(GFP_KERNEL, "Erasing effect, type %d, id %d\n", effect->type, id); + struct klgd_command *c = klgd_alloc_cmd(strlen(text) + 1); + c->bytes = text; + return c; +} + +static struct klgd_command * klgdff_start(const struct ff_effect *effect, const int id) +{ + char *text = kasprintf(GFP_KERNEL, "Playing effect, type %d, id %d\n", effect->type, id); + struct klgd_command *c = klgd_alloc_cmd(strlen(text) + 1); + c->bytes = text; + return c; +} + +static struct klgd_command * klgdff_stop(const struct ff_effect *effect, const int id) +{ + char *text = kasprintf(GFP_KERNEL, "Stopping effect, type %d, id %d\n", effect->type, id); + struct klgd_command *c = klgd_alloc_cmd(strlen(text) + 1); + c->bytes = text; + return c; +} + +static struct klgd_command * klgdff_upload(const struct ff_effect *effect, const int id) +{ + char *text = kasprintf(GFP_KERNEL, "Uploading effect, type %d, id %d\n", effect->type, id); + struct klgd_command *c = klgd_alloc_cmd(strlen(text) + 1); + c->bytes = text; + return c; +} + +static enum klgd_send_status klgdff_callback(void *data, const struct klgd_command_stream *s) +{ + size_t idx; + + printk(KERN_NOTICE "KLGDTM - EFF...\n"); + for (idx = 0; idx < s->count; idx++) + printk(KERN_NOTICE "KLGDTM - EFF %s\n", s->commands[idx]->bytes); + + usleep_range(7, 9); + + return KLGD_SS_DONE; +} + +static void __exit klgdff_exit(void) +{ + input_unregister_device(dev); + klgd_deinit(&klgd); + kobject_put(klgdff_obj); + printk(KERN_NOTICE "KLGD FF sample module removed\n"); +} + +static int __init klgdff_init(void) +{ + unsigned long ffbits = FFPL_EFBIT(FF_CONSTANT) | FFPL_EFBIT(FF_RUMBLE); + int ret; + + klgdff_obj = kobject_create_and_add("klgdff_obj", kernel_kobj); + if (!klgdff_obj) + return -ENOMEM; + + ret = klgd_init(&klgd, NULL, klgdff_callback, 1); + if (ret) { + printk(KERN_ERR "Cannot initialize KLGD\n"); + goto errout_klgd; + } + + dev = input_allocate_device(); + if (!dev) { + ret = -ENODEV; + printk(KERN_ERR "Cannot allocate input device\n"); + goto errout_idev; + } + dev->id.bustype = BUS_VIRTUAL; + dev->id.vendor = 0xffff; + dev->id.product = 0x8807; + dev->id.version = 0x8807; + dev->name = kasprintf(GFP_KERNEL, "KLGD-FF TestModule"); + dev->uniq = kasprintf(GFP_KERNEL, "KLGD-FF TestModule-X"); + dev->dev.parent = NULL; + + input_set_capability(dev, EV_ABS, ABS_X); + input_set_capability(dev, EV_ABS, ABS_Y); + input_set_capability(dev, EV_KEY, BTN_0); + input_set_capability(dev, EV_KEY, BTN_TRIGGER); + input_set_abs_params(dev, ABS_X, -0x7fff, 0x7fff, 0, 0); + input_set_abs_params(dev, ABS_Y, -0x7fff, 0x7fff, 0, 0); + + ret = ffpl_init_plugin(&ff_plugin, dev, EFFECT_COUNT, ffbits, + klgdff_upload, klgdff_start, klgdff_stop, klgdff_erase); + if (ret) { + printk(KERN_ERR "KLGDFF: Cannot init plugin\n"); + goto errout_idev; + } + ret = input_register_device(dev); + if (ret) { + printk(KERN_ERR "Cannot register input device\n"); + goto errout_regdev; + } + + ret = klgd_register_plugin(&klgd, 0, ff_plugin); + if (ret) { + printk(KERN_ERR "KLGDFF: Cannot register plugin\n"); + goto errout_idev; + } + + + + printk(KERN_NOTICE "KLGD FF sample module loaded\n"); + return 0; + +errout_regdev: + input_free_device(dev); +errout_idev: + klgd_deinit(&klgd); +errout_klgd: + kobject_put(klgdff_obj); + return ret; +} + +module_exit(klgdff_exit) +module_init(klgdff_init); + +MODULE_LICENSE("GPL"); +MODULE_AUTHOR("Michal \"MadCatX\" Maly"); +MODULE_DESCRIPTION("KLGD FF TestModule"); + + diff --git a/klgdtm.c b/klgdtm.c deleted file mode 100644 index 0a8feaf..0000000 --- a/klgdtm.c +++ /dev/null @@ -1,211 +0,0 @@ -#include -#include -#include -#include -#include -#include "../KLGD/klgd.h" - -MODULE_LICENSE("GPL"); -MODULE_AUTHOR("Michal \"MadCatX\" Maly"); -MODULE_DESCRIPTION("..."); - -/* BEGIN: Forward function declarations */ -static ssize_t klgdtm_num_show(struct kobject *kobj, struct kobj_attribute *attr, char *buf); -static ssize_t klgdtm_num_store(struct kobject *kobj, struct kobj_attribute *attr, const char *buf, size_t count); -/* END: Forward function declarations */ - -/* BEGIN: Variables used by the module */ -static struct kobject *klgdtm_obj; -static int klgdtm_number; - -static struct klgd_main klgd; -static struct timer_list klgdtm_timer; - -static struct kobj_attribute put_num_attr = - __ATTR(num, 0666, klgdtm_num_show, klgdtm_num_store); - -static struct attribute *attrs[] = { - &put_num_attr.attr, - NULL -}; - -static struct attribute_group attrs_grp = { - .attrs = attrs -}; -/* END: Variables used by the module */ - -/* BEGIN: KLGDTM test plugin definition */ - -struct klgd_plugin_private { - int old_n; - int n; -}; - -static struct klgd_plugin_private plugin_private = { - 0, - 0 -}; - -static struct klgd_command_stream * klgdtm_plugin_get_commands(struct klgd_plugin *self, const unsigned long now) -{ - struct klgd_plugin_private *p = self->private; - struct klgd_command_stream *s; - struct klgd_command *c; - - s = kzalloc(sizeof(struct klgd_command_stream), GFP_KERNEL); - if (!s) - return NULL; - s->commands = kzalloc(sizeof(struct klgd_command *) * 2, GFP_KERNEL); - if (!s->commands) { - kfree(s); - return NULL; - } - - c = kzalloc(sizeof(struct klgd_command), GFP_KERNEL); - if (!c) - return NULL; - - c->bytes = kzalloc(sizeof(int), GFP_KERNEL); - if (!c->bytes) { - kfree(s->commands); - kfree(s); - kfree(c); - return NULL; - } - c->length = sizeof(int); - - *(int*)(c->bytes) = p->n; - *(int*)(c->bytes + sizeof(int)) = p->old_n; - - s->commands[0] = c; - s->count = 1; - - p->old_n = p->n; - - return s; -} - -bool klgdtm_plugin_get_update_time(struct klgd_plugin *self, const unsigned long now, unsigned long *t) -{ - struct klgd_plugin_private *p = self->private; - - if (p->n) { - *t = now + msecs_to_jiffies(500); - return true; - } - return false; -} - - -int klgdtm_plugin_post_event(struct klgd_plugin *self, void *data) -{ - struct klgd_plugin_private *p = self->private; - int num = *(int*)data; - - p->n = num; - if (num == 0) - p->old_n = 0; - - return 0; -} - -static struct klgd_plugin klgdtm_plugin = { - &plugin_private, - NULL, - klgdtm_plugin_get_commands, - klgdtm_plugin_get_update_time, - NULL, - NULL, - klgdtm_plugin_post_event -}; - - -/* END: KLGDTM test plugin definition */ - -/* BEGIN: KLGDTM module code */ - -/* Callback function called by KLGD when a new command stream is submitted */ - -static void klgdtm_timer_fired(unsigned long data) -{ - klgd_notify_commands_sent(&klgd); -} - -static enum klgd_send_status klgdtm_callback(void *data, struct klgd_command_stream *s) -{ - int num, old_num; - if (!s) { - printk(KERN_NOTICE "Empty stream\n"); - return KLGD_SS_DONE; - } - if (s->commands[0]->length != sizeof(int)) { - printk(KERN_WARNING "Malformed stream\n"); - return KLGD_SS_FAILED; - } - - num = *(int*)s->commands[0]->bytes; - old_num = *(int*)(s->commands[0]->bytes + sizeof(int)); - printk(KERN_NOTICE "KLGDTM: Value %d, prev: %d\n", num, old_num); - - mod_timer(&klgdtm_timer, msecs_to_jiffies(1000)); - - return KLGD_SS_RUNNING; -} - -static ssize_t klgdtm_num_show(struct kobject *kobj, struct kobj_attribute *attr, char *buf) -{ - return scnprintf(buf, PAGE_SIZE, "%d\n", klgdtm_number); -} - -static ssize_t klgdtm_num_store(struct kobject *kobj, struct kobj_attribute *attr, const char *buf, size_t count) -{ - int n; - sscanf(buf, "%d", &n); - klgdtm_number = n; - - klgd_post_event(&klgd, 0, &n); - return count; -} - -static void __exit klgdtm_exit(void) -{ - sysfs_remove_group(klgdtm_obj, &attrs_grp); - klgd_deinit(&klgd); - kobject_put(klgdtm_obj); - printk(KERN_NOTICE "KLGD sample module removed\n"); -} - -static int __init klgdtm_init(void) -{ - int ret; - - klgdtm_obj = kobject_create_and_add("klgdtm_obj", kernel_kobj); - if (!klgdtm_obj) - return -ENOMEM; - - ret = sysfs_create_group(klgdtm_obj, &attrs_grp); - if (ret) - goto errout_sysfs; - - ret = klgd_init(&klgd, NULL, klgdtm_callback, 1); - if (ret) { - printk(KERN_ERR "Cannot initialize KLGD\n"); - goto errout_klgd; - } - - klgd_register_plugin(&klgd, 0, &klgdtm_plugin); - setup_timer(&klgdtm_timer, klgdtm_timer_fired, 0); - - printk(KERN_NOTICE "KLGD sample module loaded\n"); - return 0; - -errout_klgd: - sysfs_remove_group(klgdtm_obj, &attrs_grp); -errout_sysfs: - kobject_put(klgdtm_obj); - return ret; -} -/* END: KLGDTM module code */ - -module_exit(klgdtm_exit) -module_init(klgdtm_init);