From c6a4a40af218a9bc36cb3f979ca7f65e1f0af6d1 Mon Sep 17 00:00:00 2001 From: Dmitry Domnin Date: Wed, 25 Dec 2019 16:50:37 +0200 Subject: [PATCH] 11-InterruptsHandling: Fix module Signed-off-by: Dmitry Domnin --- 11-InterruptsHandling/mpu6050.c | 427 +++++++++++++++++++------------- 1 file changed, 251 insertions(+), 176 deletions(-) diff --git a/11-InterruptsHandling/mpu6050.c b/11-InterruptsHandling/mpu6050.c index b6ef64b..e8572be 100644 --- a/11-InterruptsHandling/mpu6050.c +++ b/11-InterruptsHandling/mpu6050.c @@ -4,293 +4,368 @@ #include #include #include - +#include +#include +#include +#include #include "mpu6050-regs.h" +struct temp_t { + int t_int; + int t_frac; +}; struct mpu6050_data { struct i2c_client *drv_client; int accel_values[3]; int gyro_values[3]; - int temperature; + struct temp_t temperature; + struct mutex mutex_mpu; }; static struct mpu6050_data g_mpu6050_data; static int mpu6050_read_data(void) { - int temp; + int acc_x = 0, acc_y = 0, acc_z = 0; + int gyro_x = 0, gyro_y = 0, gyro_z = 0; + int temp = 0, ret = 0, i = 0; struct i2c_client *drv_client = g_mpu6050_data.drv_client; if (drv_client == 0) return -ENODEV; - /* accel */ - g_mpu6050_data.accel_values[0] = (s16)((u16)i2c_smbus_read_word_swapped(drv_client, REG_ACCEL_XOUT_H)); - g_mpu6050_data.accel_values[1] = (s16)((u16)i2c_smbus_read_word_swapped(drv_client, REG_ACCEL_YOUT_H)); - g_mpu6050_data.accel_values[2] = (s16)((u16)i2c_smbus_read_word_swapped(drv_client, REG_ACCEL_ZOUT_H)); - /* gyro */ - g_mpu6050_data.gyro_values[0] = (s16)((u16)i2c_smbus_read_word_swapped(drv_client, REG_GYRO_XOUT_H)); - g_mpu6050_data.gyro_values[1] = (s16)((u16)i2c_smbus_read_word_swapped(drv_client, REG_GYRO_YOUT_H)); - g_mpu6050_data.gyro_values[2] = (s16)((u16)i2c_smbus_read_word_swapped(drv_client, REG_GYRO_ZOUT_H)); - /* Temperature in degrees C = - * (TEMP_OUT Register Value as a signed quantity)/340 + 36.53 - */ - temp = (s16)((u16)i2c_smbus_read_word_swapped(drv_client, REG_TEMP_OUT_H)); - g_mpu6050_data.temperature = (temp + 12420 + 170) / 340; - - 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.accel_values[0], - g_mpu6050_data.accel_values[1], - g_mpu6050_data.accel_values[2]); - dev_info(&drv_client->dev, "GYRO[X,Y,Z] = [%d, %d, %d]\n", - g_mpu6050_data.gyro_values[0], - g_mpu6050_data.gyro_values[1], - g_mpu6050_data.gyro_values[2]); - dev_info(&drv_client->dev, "TEMP = %d\n", - g_mpu6050_data.temperature); - - return 0; -} - -static int mpu6050_probe(struct i2c_client *drv_client, - const struct i2c_device_id *id) -{ - int ret; - - dev_info(&drv_client->dev, - "i2c client address is 0x%X\n", drv_client->addr); + // Disable INT + if (IS_ERR_VALUE(i2c_smbus_write_byte_data(drv_client, + REG_INT_ENABLE, 0x00))) { + dev_err(&drv_client->dev, + "i2c_smbus_write_byte_data() failed\n"); + return -EIO; + } - /* Read who_am_i register */ - ret = i2c_smbus_read_byte_data(drv_client, REG_WHO_AM_I); + ret = i2c_smbus_read_byte_data(drv_client, REG_INT_STATUS); if (IS_ERR_VALUE(ret)) { dev_err(&drv_client->dev, - "i2c_smbus_read_byte_data() failed with error: %d\n", - ret); - return ret; + "i2c_smbus_read_byte_data() failed\n"); + return -EIO; } - if (ret != MPU6050_WHO_AM_I) { - dev_err(&drv_client->dev, - "wrong i2c device found: expected 0x%X, found 0x%X\n", - MPU6050_WHO_AM_I, ret); - return -1; + dev_info(&drv_client->dev, "REG_INT_STATUS: 0x%x\n", ret); + + if (ret & 0x40) { + for (i = 0; i < 10; i++) { + /* accel */ + acc_x += (s16)((u16)i2c_smbus_read_word_swapped(drv_client, REG_ACCEL_XOUT_H)); + acc_y += (s16)((u16)i2c_smbus_read_word_swapped(drv_client, REG_ACCEL_YOUT_H)); + acc_z += (s16)((u16)i2c_smbus_read_word_swapped(drv_client, REG_ACCEL_ZOUT_H)); + /* gyro */ + gyro_x += (s16)((u16)i2c_smbus_read_word_swapped(drv_client, REG_GYRO_XOUT_H)); + gyro_y += (s16)((u16)i2c_smbus_read_word_swapped(drv_client, REG_GYRO_YOUT_H)); + gyro_z += (s16)((u16)i2c_smbus_read_word_swapped(drv_client, REG_GYRO_ZOUT_H)); + /* temp */ + temp += (s16)((u16)i2c_smbus_read_word_swapped(drv_client, REG_TEMP_OUT_H)); + + msleep(1); + } + g_mpu6050_data.accel_values[0] = acc_x / 10; + g_mpu6050_data.accel_values[1] = acc_y / 10; + g_mpu6050_data.accel_values[2] = acc_z / 10; + g_mpu6050_data.gyro_values[0] = gyro_x / 10; + g_mpu6050_data.gyro_values[1] = gyro_y / 10; + g_mpu6050_data.gyro_values[2] = gyro_z / 10; + + g_mpu6050_data.temperature.t_int = (temp / 10 * 1000 / 340 + 35000) / 1000; + g_mpu6050_data.temperature.t_frac = (temp / 10 * 1000 / 340 + 35000) % 1000; + + 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.accel_values[0], + g_mpu6050_data.accel_values[1], + g_mpu6050_data.accel_values[2]); + dev_info(&drv_client->dev, "GYRO[X,Y,Z] = [%d, %d, %d]\n", + g_mpu6050_data.gyro_values[0], + g_mpu6050_data.gyro_values[1], + g_mpu6050_data.gyro_values[2]); + dev_info(&drv_client->dev, "TEMP = %02d.%03d\n", + g_mpu6050_data.temperature.t_int, + g_mpu6050_data.temperature.t_frac); } - dev_info(&drv_client->dev, - "i2c mpu6050 device found, WHO_AM_I register value = 0x%X\n", - ret); - - /* Setup the device */ - /* No error handling here! */ - i2c_smbus_write_byte_data(drv_client, REG_CONFIG, 0); - i2c_smbus_write_byte_data(drv_client, REG_GYRO_CONFIG, 0); - i2c_smbus_write_byte_data(drv_client, REG_ACCEL_CONFIG, 0); - i2c_smbus_write_byte_data(drv_client, REG_FIFO_EN, 0); - i2c_smbus_write_byte_data(drv_client, REG_INT_PIN_CFG, 0); - i2c_smbus_write_byte_data(drv_client, REG_INT_ENABLE, 0); - i2c_smbus_write_byte_data(drv_client, REG_USER_CTRL, 0); - i2c_smbus_write_byte_data(drv_client, REG_PWR_MGMT_1, 0); - i2c_smbus_write_byte_data(drv_client, REG_PWR_MGMT_2, 0); - g_mpu6050_data.drv_client = drv_client; + // Enable INT + if (IS_ERR_VALUE(i2c_smbus_write_byte_data(drv_client, + REG_INT_ENABLE, 0x40))) { + dev_err(&drv_client->dev, + "i2c_smbus_write_byte_data() failed\n"); + return -EIO; + } - dev_info(&drv_client->dev, "i2c driver probed\n"); return 0; } -static int mpu6050_remove(struct i2c_client *drv_client) +static irqreturn_t interrupt_handler(int irq, void *dev_id) { - g_mpu6050_data.drv_client = 0; - - dev_info(&drv_client->dev, "i2c driver removed\n"); - return 0; + dev_info(&((struct mpu6050_data *)dev_id)->drv_client->dev, + "Interrupt #%d", + ((struct mpu6050_data *)dev_id)->drv_client->irq); + return IRQ_WAKE_THREAD; } -static const struct i2c_device_id mpu6050_idtable[] = { - { "mpu6050", 0 }, - { } -}; -MODULE_DEVICE_TABLE(i2c, mpu6050_idtable); - -static struct i2c_driver mpu6050_i2c_driver = { - .driver = { - .name = "gl_mpu6050", - }, +static irqreturn_t interrupt_thread(int irq, void *dev_id) +{ + mutex_lock(&((struct mpu6050_data *)dev_id)->mutex_mpu); + mpu6050_read_data(); + mutex_unlock(&((struct mpu6050_data *)dev_id)->mutex_mpu); - .probe = mpu6050_probe, - .remove = mpu6050_remove, - .id_table = mpu6050_idtable, -}; + return IRQ_HANDLED; +} static ssize_t accel_x_show(struct class *class, struct class_attribute *attr, char *buf) { - mpu6050_read_data(); - + mutex_lock(&g_mpu6050_data.mutex_mpu); sprintf(buf, "%d\n", g_mpu6050_data.accel_values[0]); + mutex_unlock(&g_mpu6050_data.mutex_mpu); + return strlen(buf); } static ssize_t accel_y_show(struct class *class, struct class_attribute *attr, char *buf) { - mpu6050_read_data(); - + mutex_lock(&g_mpu6050_data.mutex_mpu); sprintf(buf, "%d\n", g_mpu6050_data.accel_values[1]); + mutex_unlock(&g_mpu6050_data.mutex_mpu); + return strlen(buf); } static ssize_t accel_z_show(struct class *class, struct class_attribute *attr, char *buf) { - mpu6050_read_data(); - + mutex_lock(&g_mpu6050_data.mutex_mpu); sprintf(buf, "%d\n", g_mpu6050_data.accel_values[2]); + mutex_unlock(&g_mpu6050_data.mutex_mpu); + return strlen(buf); } static ssize_t gyro_x_show(struct class *class, struct class_attribute *attr, char *buf) { - mpu6050_read_data(); - + mutex_lock(&g_mpu6050_data.mutex_mpu); sprintf(buf, "%d\n", g_mpu6050_data.gyro_values[0]); + mutex_unlock(&g_mpu6050_data.mutex_mpu); + return strlen(buf); } static ssize_t gyro_y_show(struct class *class, struct class_attribute *attr, char *buf) { - mpu6050_read_data(); - + mutex_lock(&g_mpu6050_data.mutex_mpu); sprintf(buf, "%d\n", g_mpu6050_data.gyro_values[1]); + mutex_unlock(&g_mpu6050_data.mutex_mpu); + return strlen(buf); } static ssize_t gyro_z_show(struct class *class, struct class_attribute *attr, char *buf) { - mpu6050_read_data(); - + mutex_lock(&g_mpu6050_data.mutex_mpu); sprintf(buf, "%d\n", g_mpu6050_data.gyro_values[2]); + mutex_unlock(&g_mpu6050_data.mutex_mpu); + return strlen(buf); } static ssize_t temp_show(struct class *class, struct class_attribute *attr, char *buf) { - mpu6050_read_data(); + mutex_lock(&g_mpu6050_data.mutex_mpu); + sprintf(buf, "%02d.%03d\n", g_mpu6050_data.temperature.t_int, + g_mpu6050_data.temperature.t_frac); + mutex_unlock(&g_mpu6050_data.mutex_mpu); - sprintf(buf, "%d\n", g_mpu6050_data.temperature); return strlen(buf); } -CLASS_ATTR(accel_x, 0444, &accel_x_show, NULL); -CLASS_ATTR(accel_y, 0444, &accel_y_show, NULL); -CLASS_ATTR(accel_z, 0444, &accel_z_show, NULL); -CLASS_ATTR(gyro_x, 0444, &gyro_x_show, NULL); -CLASS_ATTR(gyro_y, 0444, &gyro_y_show, NULL); -CLASS_ATTR(gyro_z, 0444, &gyro_z_show, NULL); -CLASS_ATTR(temperature, 0444, &temp_show, NULL); +CLASS_ATTR_RO(accel_x); +CLASS_ATTR_RO(accel_y); +CLASS_ATTR_RO(accel_z); +CLASS_ATTR_RO(gyro_x); +CLASS_ATTR_RO(gyro_y); +CLASS_ATTR_RO(gyro_z); +CLASS_ATTR_RO(temp); + +static struct attribute *mpu_class_attrs[] = { + &class_attr_accel_x.attr, + &class_attr_accel_y.attr, + &class_attr_accel_z.attr, + &class_attr_gyro_x.attr, + &class_attr_gyro_y.attr, + &class_attr_gyro_z.attr, + &class_attr_temp.attr, + NULL, +}; + +ATTRIBUTE_GROUPS(mpu_class); -static struct class *attr_class; +/* Device model classes */ +struct class mpu_rc_class = { + .name = "mpu6050", + .owner = THIS_MODULE, + .class_groups = mpu_class_groups, +}; -static int mpu6050_init(void) +static int mpu6050_probe(struct i2c_client *drv_client, + const struct i2c_device_id *id) { int ret; - /* Create i2c driver */ - ret = i2c_add_driver(&mpu6050_i2c_driver); - if (ret) { - pr_err("mpu6050: failed to add new i2c driver: %d\n", ret); + ret = class_register(&mpu_rc_class); + if (ret < 0) { + dev_err(&drv_client->dev, "failed to create sysfs class: %d\n", + ret); return ret; } - pr_info("mpu6050: i2c driver created\n"); + dev_info(&drv_client->dev, "sysfs class created\n"); - /* Create class */ - attr_class = class_create(THIS_MODULE, "mpu6050"); - if (IS_ERR(attr_class)) { - ret = PTR_ERR(attr_class); - pr_err("mpu6050: failed to create sysfs class: %d\n", ret); - return ret; + dev_info(&drv_client->dev, + "i2c client address is 0x%X\n", drv_client->addr); + + /* Read who_am_i register */ + ret = i2c_smbus_read_byte_data(drv_client, REG_WHO_AM_I); + if (IS_ERR_VALUE(ret)) { + dev_err(&drv_client->dev, + "i2c_smbus_read_byte_data() failed\n"); + return -EIO; } - pr_info("mpu6050: sysfs class created\n"); + if (ret != MPU6050_WHO_AM_I) { + dev_err(&drv_client->dev, + "wrong i2c device found: expected 0x%X, found 0x%X\n", + MPU6050_WHO_AM_I, ret); + return -1; + } + dev_info(&drv_client->dev, + "i2c mpu6050 device found, WHO_AM_I register value = 0x%X\n", + ret); - /* Create accel_x */ - ret = class_create_file(attr_class, &class_attr_accel_x); - if (ret) { - pr_err("mpu6050: failed to create sysfs class attribute accel_x: %d\n", ret); - return ret; + /* Setup the device */ + // reset + if (IS_ERR_VALUE(i2c_smbus_write_byte_data(drv_client, + REG_PWR_MGMT_1, 0x80))) { + dev_err(&drv_client->dev, + "i2c_smbus_write_byte_data() failed\n"); + return -EIO; } - /* Create accel_y */ - ret = class_create_file(attr_class, &class_attr_accel_y); - if (ret) { - pr_err("mpu6050: failed to create sysfs class attribute accel_y: %d\n", ret); - return ret; + + msleep(100); + + // start + if (IS_ERR_VALUE(i2c_smbus_write_byte_data(drv_client, + REG_PWR_MGMT_1, 0x00))) { + dev_err(&drv_client->dev, + "i2c_smbus_write_byte_data() failed\n"); + return -EIO; } - /* Create accel_z */ - ret = class_create_file(attr_class, &class_attr_accel_z); - if (ret) { - pr_err("mpu6050: failed to create sysfs class attribute accel_z: %d\n", ret); - return ret; + if (IS_ERR_VALUE(i2c_smbus_write_byte_data(drv_client, + REG_CONFIG, 0x03))) { + dev_err(&drv_client->dev, + "i2c_smbus_write_byte_data() failed\n"); + return -EIO; } - /* Create gyro_x */ - ret = class_create_file(attr_class, &class_attr_gyro_x); - if (ret) { - pr_err("mpu6050: failed to create sysfs class attribute gyro_x: %d\n", ret); - return ret; + if (IS_ERR_VALUE(i2c_smbus_write_byte_data(drv_client, + REG_SMPRT_DIV, 0x04))) { + dev_err(&drv_client->dev, + "i2c_smbus_write_byte_data() failed\n"); + return -EIO; } - /* Create gyro_y */ - ret = class_create_file(attr_class, &class_attr_gyro_y); - if (ret) { - pr_err("mpu6050: failed to create sysfs class attribute gyro_y: %d\n", ret); - return ret; + if (IS_ERR_VALUE(i2c_smbus_write_byte_data(drv_client, + REG_MOT_THR, 0x15))) { + dev_err(&drv_client->dev, + "i2c_smbus_write_byte_data() failed\n"); + return -EIO; } - /* Create gyro_z */ - ret = class_create_file(attr_class, &class_attr_gyro_z); - if (ret) { - pr_err("mpu6050: failed to create sysfs class attribute gyro_z: %d\n", ret); - return ret; + if (IS_ERR_VALUE(i2c_smbus_write_byte_data(drv_client, + REG_MOT_DUR, 0x02))) { + dev_err(&drv_client->dev, + "i2c_smbus_write_byte_data() failed\n"); + return -EIO; + } + if (IS_ERR_VALUE(i2c_smbus_write_byte_data(drv_client, + REG_INT_PIN_CFG, 0x82))) { + dev_err(&drv_client->dev, + "i2c_smbus_write_byte_data() failed\n"); + return -EIO; + } + if (IS_ERR_VALUE(i2c_smbus_write_byte_data(drv_client, + REG_INT_ENABLE, 0x40))) { + dev_err(&drv_client->dev, + "i2c_smbus_write_byte_data() failed\n"); + return -EIO; } - /* Create temperature */ - ret = class_create_file(attr_class, &class_attr_temperature); + + mutex_init(&g_mpu6050_data.mutex_mpu); + + ret = request_threaded_irq(drv_client->irq, interrupt_handler, + interrupt_thread, IRQF_ONESHOT | IRQF_TRIGGER_FALLING, + "mpu6050", (void *)&g_mpu6050_data); if (ret) { - pr_err("mpu6050: failed to create sysfs class attribute temperature: %d\n", ret); + dev_err(&drv_client->dev, + "error irq , unable to request IRQ#%d: %d\n", + drv_client->irq, ret); return ret; } - pr_info("mpu6050: sysfs class attributes created\n"); + dev_info(&drv_client->dev, "IRQ #%d\n", drv_client->irq); + + g_mpu6050_data.drv_client = drv_client; - pr_info("mpu6050: module loaded\n"); + dev_info(&drv_client->dev, "i2c driver probed\n"); return 0; } -static void mpu6050_exit(void) +static int mpu6050_remove(struct i2c_client *drv_client) { - if (attr_class) { - class_remove_file(attr_class, &class_attr_accel_x); - class_remove_file(attr_class, &class_attr_accel_y); - class_remove_file(attr_class, &class_attr_accel_z); - class_remove_file(attr_class, &class_attr_gyro_x); - class_remove_file(attr_class, &class_attr_gyro_y); - class_remove_file(attr_class, &class_attr_gyro_z); - class_remove_file(attr_class, &class_attr_temperature); - pr_info("mpu6050: sysfs class attributes removed\n"); - - class_destroy(attr_class); - pr_info("mpu6050: sysfs class destroyed\n"); - } + g_mpu6050_data.drv_client = 0; + + free_irq(drv_client->irq, (void *)drv_client); + + mutex_destroy(&g_mpu6050_data.mutex_mpu); - i2c_del_driver(&mpu6050_i2c_driver); - pr_info("mpu6050: i2c driver deleted\n"); + class_unregister(&mpu_rc_class); + dev_info(&drv_client->dev, "sysfs class destroyed\n"); - pr_info("mpu6050: module exited\n"); + dev_info(&drv_client->dev, "i2c driver removed\n"); + return 0; } -module_init(mpu6050_init); -module_exit(mpu6050_exit); +static const struct of_device_id mpu6050_ids[] = { + { .compatible = "gl, mpu6050", }, + { /* sentinel */ } +}; +MODULE_DEVICE_TABLE(of, mpu6050_ids); + +static const struct i2c_device_id mpu6050_idtable[] = { + { "mpu6050", 0 }, + { } +}; +MODULE_DEVICE_TABLE(i2c, mpu6050_idtable); + +static struct i2c_driver mpu6050_i2c_driver = { + .driver = { + .owner = THIS_MODULE, + .name = "mpu6050", + .of_match_table = of_match_ptr(mpu6050_ids), + }, + .probe = mpu6050_probe, + .remove = mpu6050_remove, + .id_table = mpu6050_idtable, +}; + +module_i2c_driver(mpu6050_i2c_driver); -MODULE_AUTHOR("Andriy.Khulap "); -MODULE_DESCRIPTION("mpu6050 I2C acc&gyro"); +MODULE_AUTHOR("Dmitry Domnin"); +MODULE_DESCRIPTION("mpu6050 I2C acc&gyro interrupt"); MODULE_LICENSE("GPL"); MODULE_VERSION("0.1");