From 7558d6fa87b692043d122718a8e5eb13d9ab988c Mon Sep 17 00:00:00 2001 From: "dmytro.kirtoka" Date: Wed, 15 Nov 2017 17:02:23 +0200 Subject: [PATCH 1/3] added buffered read data by time added ring buffer with 10 elm added timer with module parameter rate, default is 1000 msec added work for periodic read data from device sheduled by time --- mpu6050/mpu6050.c | 163 ++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 142 insertions(+), 21 deletions(-) diff --git a/mpu6050/mpu6050.c b/mpu6050/mpu6050.c index 61b541e..6e84d97 100644 --- a/mpu6050/mpu6050.c +++ b/mpu6050/mpu6050.c @@ -5,6 +5,12 @@ #include #include +#include +#include +#include +#include +#include + #include "mpu6050-regs.h" enum AXES { @@ -21,9 +27,13 @@ enum VALUE_TYPE { }; #define READ_REG_NUM (AXES_NUM * 2 + 1) +#define READ_DEPTH 10 struct mpu6050_data { struct i2c_client *drv_client; + int head; + int count; + spinlock_t lock; union { struct { int accel[AXES_NUM]; @@ -31,17 +41,56 @@ struct mpu6050_data { int gyro [AXES_NUM]; } values; int raw[READ_REG_NUM]; - } data; + } data[READ_DEPTH]; +}; + +static const struct i2c_device_id mpu6050_idtable[] = { + { "mpu6050", 0 }, + { } }; -static struct mpu6050_data g_mpu6050_data; static ssize_t show_item(struct class *class, struct class_attribute *attr, char *buf); +static void tmr_handler(unsigned long); + + +static struct mpu6050_data g_mpu6050_data; +DECLARE_COMPLETION(has_data); + +static struct work_struct read_work; + +DEFINE_TIMER( mytimer, tmr_handler, 0, 0 ); + +MODULE_DEVICE_TABLE(i2c, mpu6050_idtable); + +static struct class *attr_class; + +static int rate = 1000; +static int is_buf; + +module_param( rate, int, S_IRUGO ); +module_param( is_buf, int, S_IRUGO ); + +static inline int get_next_pos (int pos, const int depth) +{ + int tmp = pos + 1; + if (tmp >= depth) { + tmp -= depth; + } + return tmp; +} + +static void mpu6050_data_flush(void) +{ + reinit_completion(&has_data); + g_mpu6050_data.head = 0; + g_mpu6050_data.count = 0; +} static int mpu6050_read_data(void) { u8 values[READ_REG_NUM * sizeof(u16)]; - int temp, result, i, *p; + int temp, result, i, head, count, *p; struct i2c_client *drv_client = g_mpu6050_data.drv_client; if (drv_client == 0) @@ -55,33 +104,91 @@ static int mpu6050_read_data(void) return -EINVAL; } - for (i = 0, p = g_mpu6050_data.data.raw; i < READ_REG_NUM * sizeof(u16); i += 2, ++p) + head = get_next_pos(g_mpu6050_data.head, READ_DEPTH); + + for (i = 0, p = g_mpu6050_data.data[head].raw; i < READ_REG_NUM * sizeof(u16); i += 2, ++p) *p = (s16) ((u16) values[i] << 8 | (u16)values[i + 1]); + spin_lock(&g_mpu6050_data.lock); + g_mpu6050_data.head = head; + count = g_mpu6050_data.count; + g_mpu6050_data.count = count + 1; + spin_unlock(&g_mpu6050_data.lock); + + dev_info(&drv_client->dev, "sensor data read head %d, count %d\n", g_mpu6050_data.head, g_mpu6050_data.count); + /* Temperature in degrees C = * (TEMP_OUT Register Value as a signed quantity)/340 + 36.53 */ - temp = g_mpu6050_data.data.values.temperature; - g_mpu6050_data.data.values.temperature = (temp + 12420 + 170) / 340; + temp = g_mpu6050_data.data[head].values.temperature; + g_mpu6050_data.data[head].values.temperature = (temp + 12420 + 170) / 340; + + if ((is_buf && count < READ_DEPTH) || (!is_buf && !count)) + complete(&has_data); dev_info(&drv_client->dev, "sensor data read:\n"); dev_info(&drv_client->dev, "ACCEL[X,Y,Z] = [%d, %d, %d]\n", - g_mpu6050_data.data.values.accel[AXIS_X], - g_mpu6050_data.data.values.accel[AXIS_Y], - g_mpu6050_data.data.values.accel[AXIS_Z]); + g_mpu6050_data.data[head].values.accel[AXIS_X], + g_mpu6050_data.data[head].values.accel[AXIS_Y], + g_mpu6050_data.data[head].values.accel[AXIS_Z]); dev_info(&drv_client->dev, "GYRO[X,Y,Z] = [%d, %d, %d]\n", - g_mpu6050_data.data.values.gyro[AXIS_X], - g_mpu6050_data.data.values.gyro[AXIS_Y], - g_mpu6050_data.data.values.gyro[AXIS_Z]); + g_mpu6050_data.data[head].values.gyro[AXIS_X], + g_mpu6050_data.data[head].values.gyro[AXIS_Y], + g_mpu6050_data.data[head].values.gyro[AXIS_Z]); dev_info(&drv_client->dev, "TEMP = %d\n", - g_mpu6050_data.data.values.temperature); + g_mpu6050_data.data[head].values.temperature); return 0; } +static int mpu6050_get_data(int *values, int is_buffered) +{ + if (is_buffered) { + int head, count, tail, diff; + + if (wait_for_completion_interruptible(&has_data)) { + pr_info("mpu6050: interrupted\n"); + return 0; + } + + spin_lock(&g_mpu6050_data.lock); + head = g_mpu6050_data.head; + count = g_mpu6050_data.count >= READ_DEPTH ? READ_DEPTH : g_mpu6050_data.count; + g_mpu6050_data.count = count ? count - 1 : 0; + spin_unlock(&g_mpu6050_data.lock); + + if (!count) { + pr_err("mpu6050: bad flow\n"); + return 0; + } + + diff = head - count + 1; + tail = diff < 0 ? READ_DEPTH - diff : diff; + memcpy(values, g_mpu6050_data.data[tail].raw, sizeof(int) * READ_REG_NUM); + pr_info("mpu6050: get data copied tail %d count %d\n", tail, count); + } + else { + if (wait_for_completion_interruptible(&has_data)) { + pr_info("mpu6050: get data interrupted\n"); + return 0; + } + memcpy(values, g_mpu6050_data.data[g_mpu6050_data.head].raw, sizeof(int) * READ_REG_NUM); + spin_lock(&g_mpu6050_data.lock); + g_mpu6050_data.count = 0; + spin_unlock(&g_mpu6050_data.lock); + pr_info("mpu6050: get data copied tail %d\n", g_mpu6050_data.head); + } + return 1; +} + +static void update_values(struct work_struct *unused) +{ + mpu6050_read_data(); +} + static int mpu6050_probe(struct i2c_client *drv_client, const struct i2c_device_id *id) { @@ -134,11 +241,6 @@ static int mpu6050_remove(struct i2c_client *drv_client) return 0; } -static const struct i2c_device_id mpu6050_idtable[] = { - { "mpu6050", 0 }, - { } -}; -MODULE_DEVICE_TABLE(i2c, mpu6050_idtable); static struct i2c_driver mpu6050_i2c_driver = { .driver = { @@ -165,19 +267,30 @@ struct class_attribute class_attr_array[READ_REG_NUM] = { static ssize_t show_item(struct class *class, struct class_attribute *attr, char *buf) { int index = attr - class_attr_array; + int data[READ_REG_NUM] = {0}; int value = 0; - mpu6050_read_data(); + if (!mpu6050_get_data( data, is_buf)) + return 0; if (index >=0 && index < READ_REG_NUM) { - value = g_mpu6050_data.data.raw[index]; + value = data[index]; } sprintf(buf, "%d\n", value); return strlen(buf); } -static struct class *attr_class; +static void start_timer(void) +{ + mod_timer(&mytimer, jiffies + rate); +} + +static void tmr_handler(unsigned long ticks) +{ + start_timer(); + schedule_work(&read_work); +} static int mpu6050_init(void) { @@ -190,6 +303,8 @@ static int mpu6050_init(void) return ret; } pr_info("mpu6050: i2c driver created\n"); + spin_lock_init(&g_mpu6050_data.lock); + mpu6050_data_flush(); /* Create class */ attr_class = class_create(THIS_MODULE, "mpu6050"); @@ -209,6 +324,9 @@ static int mpu6050_init(void) } pr_info("mpu6050: sysfs class attributes created\n"); + INIT_WORK(&read_work, update_values); + start_timer(); + pr_info("mpu6050: timed read started\n"); pr_info("mpu6050: module loaded\n"); return 0; @@ -216,6 +334,9 @@ static int mpu6050_init(void) static void mpu6050_exit(void) { + del_timer(&mytimer); + cancel_work_sync(&read_work); + if (attr_class) { int i; From e9b308b17b77281f79ade3a804d7af608eb5d709 Mon Sep 17 00:00:00 2001 From: "dmytro.kirtoka" Date: Tue, 21 Nov 2017 13:35:39 +0200 Subject: [PATCH 2/3] added cdev with RO acces added dinamic add/remove device in /dev added support read only fops for mpu6050 module limitation: read data by only READ_REG_NUM blocks supported only one device --- mpu6050/mpu6050.c | 61 ++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 60 insertions(+), 1 deletion(-) diff --git a/mpu6050/mpu6050.c b/mpu6050/mpu6050.c index 6e84d97..beb5335 100644 --- a/mpu6050/mpu6050.c +++ b/mpu6050/mpu6050.c @@ -1,10 +1,11 @@ #include #include #include +#include #include #include #include - +#include #include #include #include @@ -65,9 +66,13 @@ MODULE_DEVICE_TABLE(i2c, mpu6050_idtable); static struct class *attr_class; +static struct cdev my_dev; + static int rate = 1000; static int is_buf; +static int major; +module_param( major, int, S_IRUGO ); module_param( rate, int, S_IRUGO ); module_param( is_buf, int, S_IRUGO ); @@ -252,6 +257,33 @@ static struct i2c_driver mpu6050_i2c_driver = { .id_table = mpu6050_idtable, }; +static ssize_t dev_read( struct file * file, char * buf, size_t count, loff_t *ppos ) +{ + int res; + int data[READ_REG_NUM]; + + if( count != READ_REG_NUM * sizeof(int) ) + return -EINVAL; + + res = mpu6050_get_data(data, is_buf); + + if (res != 1) + { + printk("read result %d\n", res); + return -EIO; + } + + if( copy_to_user( buf, data, READ_REG_NUM * sizeof(int) ) ) + return -EINVAL; + + return READ_REG_NUM * sizeof(int); +} + +static const struct file_operations dev_fops = { + .owner = THIS_MODULE, + .read = dev_read, +}; + struct class_attribute class_attr_array[READ_REG_NUM] = { { .attr = { .name = "accel_x", .mode = S_IRUGO }, .show = &show_item, }, { .attr = { .name = "accel_y", .mode = S_IRUGO }, .show = &show_item, }, @@ -295,6 +327,7 @@ static void tmr_handler(unsigned long ticks) static int mpu6050_init(void) { int ret, i; + dev_t dev; /* Create i2c driver */ ret = i2c_add_driver(&mpu6050_i2c_driver); @@ -306,6 +339,23 @@ static int mpu6050_init(void) spin_lock_init(&g_mpu6050_data.lock); mpu6050_data_flush(); + ret = alloc_chrdev_region(&dev, 0, 1, "gl_mpu6050"); + major = MAJOR(dev); + if( ret < 0 ) { + pr_err("mpu6050: can not register char device\n"); + return ret; + } + + cdev_init(&my_dev, &dev_fops); + my_dev.owner = THIS_MODULE; + + ret = cdev_add(&my_dev, dev, 1); + if( ret < 0 ) { + unregister_chrdev_region( MKDEV( major, 0 ), 1 ); + pr_err("mpu6050: can not add char device\n" ); + return ret; + } + /* Create class */ attr_class = class_create(THIS_MODULE, "mpu6050"); if (IS_ERR(attr_class)) { @@ -315,6 +365,8 @@ static int mpu6050_init(void) } pr_info("mpu6050: sysfs class created\n"); + device_create(attr_class, NULL, dev, NULL, "mpu6050_0" ); + for (i = 0; i < READ_REG_NUM; ++i) { ret = class_create_file(attr_class, &class_attr_array[i]); if (ret) { @@ -324,6 +376,7 @@ static int mpu6050_init(void) } pr_info("mpu6050: sysfs class attributes created\n"); + INIT_WORK(&read_work, update_values); start_timer(); pr_info("mpu6050: timed read started\n"); @@ -339,14 +392,20 @@ static void mpu6050_exit(void) if (attr_class) { int i; + dev_t dev = MKDEV( major, 0 ); for (i = 0; i < READ_REG_NUM; ++i) { class_remove_file(attr_class, &class_attr_array[i]); } pr_info("mpu6050: sysfs class attributes removed\n"); + device_destroy( attr_class, dev ); + class_destroy(attr_class); pr_info("mpu6050: sysfs class destroyed\n"); + + cdev_del( &my_dev ); + unregister_chrdev_region( MKDEV( major, 0 ), 1 ); } i2c_del_driver(&mpu6050_i2c_driver); From 2374c336d672d39a06f63aecce0144bdacc0cbab Mon Sep 17 00:00:00 2001 From: "dmytro.kirtoka" Date: Tue, 12 Dec 2017 10:58:23 +0200 Subject: [PATCH 3/3] mpu6050: apply kernel code style Signed-off-by: dmytro.kirtoka --- mpu6050/mpu6050.c | 116 ++++++++++++++++++++++++++-------------------- 1 file changed, 65 insertions(+), 51 deletions(-) diff --git a/mpu6050/mpu6050.c b/mpu6050/mpu6050.c index beb5335..e5812c4 100644 --- a/mpu6050/mpu6050.c +++ b/mpu6050/mpu6050.c @@ -39,7 +39,7 @@ struct mpu6050_data { struct { int accel[AXES_NUM]; int temperature; - int gyro [AXES_NUM]; + int gyro[AXES_NUM]; } values; int raw[READ_REG_NUM]; } data[READ_DEPTH]; @@ -51,7 +51,8 @@ static const struct i2c_device_id mpu6050_idtable[] = { }; -static ssize_t show_item(struct class *class, struct class_attribute *attr, char *buf); +static ssize_t show_item(struct class *class, + struct class_attribute *attr, char *buf); static void tmr_handler(unsigned long); @@ -60,7 +61,7 @@ DECLARE_COMPLETION(has_data); static struct work_struct read_work; -DEFINE_TIMER( mytimer, tmr_handler, 0, 0 ); +DEFINE_TIMER(mytimer, tmr_handler, 0, 0); MODULE_DEVICE_TABLE(i2c, mpu6050_idtable); @@ -72,16 +73,17 @@ static int rate = 1000; static int is_buf; static int major; -module_param( major, int, S_IRUGO ); -module_param( rate, int, S_IRUGO ); -module_param( is_buf, int, S_IRUGO ); +module_param(major, int, 0444); +module_param(rate, int, 0444); +module_param(is_buf, int, 0444); -static inline int get_next_pos (int pos, const int depth) +static inline int get_next_pos(int pos, const int depth) { int tmp = pos + 1; - if (tmp >= depth) { + + if (tmp >= depth) tmp -= depth; - } + return tmp; } @@ -102,16 +104,19 @@ static int mpu6050_read_data(void) return -ENODEV; result = i2c_smbus_read_i2c_block_data(drv_client, REG_ACCEL_XOUT_H, - READ_REG_NUM * sizeof(u16), (char*) values); + READ_REG_NUM * sizeof(u16), (char *) values); - if(result != READ_REG_NUM * sizeof(u16)) { - dev_err(&drv_client->dev, "i2c_smbus_read_i2c_block_data wrong %d\n", result); + if (result != READ_REG_NUM * sizeof(u16)) { + dev_err(&drv_client->dev, + "i2c_smbus_read_i2c_block_data wrong %d\n", + result); return -EINVAL; } head = get_next_pos(g_mpu6050_data.head, READ_DEPTH); - for (i = 0, p = g_mpu6050_data.data[head].raw; i < READ_REG_NUM * sizeof(u16); i += 2, ++p) + for (i = 0, p = g_mpu6050_data.data[head].raw; + i < READ_REG_NUM * sizeof(u16); i += 2, ++p) *p = (s16) ((u16) values[i] << 8 | (u16)values[i + 1]); spin_lock(&g_mpu6050_data.lock); @@ -120,13 +125,15 @@ static int mpu6050_read_data(void) g_mpu6050_data.count = count + 1; spin_unlock(&g_mpu6050_data.lock); - dev_info(&drv_client->dev, "sensor data read head %d, count %d\n", g_mpu6050_data.head, g_mpu6050_data.count); + dev_info(&drv_client->dev, "sensor data read head %d, count %d\n", + g_mpu6050_data.head, g_mpu6050_data.count); /* Temperature in degrees C = * (TEMP_OUT Register Value as a signed quantity)/340 + 36.53 */ temp = g_mpu6050_data.data[head].values.temperature; - g_mpu6050_data.data[head].values.temperature = (temp + 12420 + 170) / 340; + g_mpu6050_data.data[head].values.temperature = + (temp + 12420 + 170) / 340; if ((is_buf && count < READ_DEPTH) || (!is_buf && !count)) complete(&has_data); @@ -161,7 +168,8 @@ static int mpu6050_get_data(int *values, int is_buffered) spin_lock(&g_mpu6050_data.lock); head = g_mpu6050_data.head; - count = g_mpu6050_data.count >= READ_DEPTH ? READ_DEPTH : g_mpu6050_data.count; + count = g_mpu6050_data.count >= READ_DEPTH ? + READ_DEPTH : g_mpu6050_data.count; g_mpu6050_data.count = count ? count - 1 : 0; spin_unlock(&g_mpu6050_data.lock); @@ -172,19 +180,22 @@ static int mpu6050_get_data(int *values, int is_buffered) diff = head - count + 1; tail = diff < 0 ? READ_DEPTH - diff : diff; - memcpy(values, g_mpu6050_data.data[tail].raw, sizeof(int) * READ_REG_NUM); - pr_info("mpu6050: get data copied tail %d count %d\n", tail, count); - } - else { + memcpy(values, g_mpu6050_data.data[tail].raw, + sizeof(int) * READ_REG_NUM); + pr_info("mpu6050: get data copied tail %d count %d\n", + tail, count); + } else { if (wait_for_completion_interruptible(&has_data)) { pr_info("mpu6050: get data interrupted\n"); return 0; } - memcpy(values, g_mpu6050_data.data[g_mpu6050_data.head].raw, sizeof(int) * READ_REG_NUM); + memcpy(values, g_mpu6050_data.data[g_mpu6050_data.head].raw, + sizeof(int) * READ_REG_NUM); spin_lock(&g_mpu6050_data.lock); g_mpu6050_data.count = 0; spin_unlock(&g_mpu6050_data.lock); - pr_info("mpu6050: get data copied tail %d\n", g_mpu6050_data.head); + pr_info("mpu6050: get data copied tail %d\n", + g_mpu6050_data.head); } return 1; } @@ -257,23 +268,23 @@ static struct i2c_driver mpu6050_i2c_driver = { .id_table = mpu6050_idtable, }; -static ssize_t dev_read( struct file * file, char * buf, size_t count, loff_t *ppos ) +static ssize_t dev_read(struct file *file, + char *buf, size_t count, loff_t *ppos) { int res; int data[READ_REG_NUM]; - if( count != READ_REG_NUM * sizeof(int) ) + if (count != READ_REG_NUM * sizeof(int)) return -EINVAL; res = mpu6050_get_data(data, is_buf); - if (res != 1) - { - printk("read result %d\n", res); + if (res != 1) { + pr_err("read result %d\n", res); return -EIO; } - if( copy_to_user( buf, data, READ_REG_NUM * sizeof(int) ) ) + if (copy_to_user(buf, data, READ_REG_NUM * sizeof(int))) return -EINVAL; return READ_REG_NUM * sizeof(int); @@ -285,29 +296,30 @@ static const struct file_operations dev_fops = { }; struct class_attribute class_attr_array[READ_REG_NUM] = { - { .attr = { .name = "accel_x", .mode = S_IRUGO }, .show = &show_item, }, - { .attr = { .name = "accel_y", .mode = S_IRUGO }, .show = &show_item, }, - { .attr = { .name = "accel_z", .mode = S_IRUGO }, .show = &show_item, }, + { .attr = { .name = "accel_x", .mode = 0444 }, .show = &show_item, }, + { .attr = { .name = "accel_y", .mode = 0444 }, .show = &show_item, }, + { .attr = { .name = "accel_z", .mode = 0444 }, .show = &show_item, }, - { .attr = { .name = "temperature", .mode = S_IRUGO }, .show = &show_item, }, + { .attr = { .name = "temperature", .mode = 0444 }, + .show = &show_item, }, - { .attr = { .name = "gyro_x", .mode = S_IRUGO }, .show = &show_item, }, - { .attr = { .name = "gyro_y", .mode = S_IRUGO }, .show = &show_item, }, - { .attr = { .name = "gyro_z", .mode = S_IRUGO }, .show = &show_item, } + { .attr = { .name = "gyro_x", .mode = 0444 }, .show = &show_item, }, + { .attr = { .name = "gyro_y", .mode = 0444 }, .show = &show_item, }, + { .attr = { .name = "gyro_z", .mode = 0444 }, .show = &show_item, } }; -static ssize_t show_item(struct class *class, struct class_attribute *attr, char *buf) +static ssize_t show_item(struct class *class, + struct class_attribute *attr, char *buf) { int index = attr - class_attr_array; int data[READ_REG_NUM] = {0}; int value = 0; - if (!mpu6050_get_data( data, is_buf)) + if (!mpu6050_get_data(data, is_buf)) return 0; - if (index >=0 && index < READ_REG_NUM) { + if (index >= 0 && index < READ_REG_NUM) value = data[index]; - } sprintf(buf, "%d\n", value); return strlen(buf); @@ -341,7 +353,7 @@ static int mpu6050_init(void) ret = alloc_chrdev_region(&dev, 0, 1, "gl_mpu6050"); major = MAJOR(dev); - if( ret < 0 ) { + if (ret < 0) { pr_err("mpu6050: can not register char device\n"); return ret; } @@ -350,9 +362,9 @@ static int mpu6050_init(void) my_dev.owner = THIS_MODULE; ret = cdev_add(&my_dev, dev, 1); - if( ret < 0 ) { - unregister_chrdev_region( MKDEV( major, 0 ), 1 ); - pr_err("mpu6050: can not add char device\n" ); + if (ret < 0) { + unregister_chrdev_region(MKDEV(major, 0), 1); + pr_err("mpu6050: can not add char device\n"); return ret; } @@ -365,12 +377,13 @@ static int mpu6050_init(void) } pr_info("mpu6050: sysfs class created\n"); - device_create(attr_class, NULL, dev, NULL, "mpu6050_0" ); + device_create(attr_class, NULL, dev, NULL, "mpu6050_0"); for (i = 0; i < READ_REG_NUM; ++i) { ret = class_create_file(attr_class, &class_attr_array[i]); if (ret) { - pr_err("mpu6050: failed to create sysfs class attribute accel_x: %d\n", ret); + pr_err("mpu6050: failed to create sysfs class: %d\n", + ret); return ret; } } @@ -392,20 +405,21 @@ static void mpu6050_exit(void) if (attr_class) { int i; - dev_t dev = MKDEV( major, 0 ); - for (i = 0; i < READ_REG_NUM; ++i) { + dev_t dev = MKDEV(major, 0); + + for (i = 0; i < READ_REG_NUM; ++i) class_remove_file(attr_class, &class_attr_array[i]); - } + pr_info("mpu6050: sysfs class attributes removed\n"); - device_destroy( attr_class, dev ); + device_destroy(attr_class, dev); class_destroy(attr_class); pr_info("mpu6050: sysfs class destroyed\n"); - cdev_del( &my_dev ); - unregister_chrdev_region( MKDEV( major, 0 ), 1 ); + cdev_del(&my_dev); + unregister_chrdev_region(MKDEV(major, 0), 1); } i2c_del_driver(&mpu6050_i2c_driver);