Modify the gsensor to support auto rotate.

gsensor.c

@@ -12,6 +12,13 @@
 #include <linux/sysfs.h>
 #include <linux/slab.h>
 #include <linux/workqueue.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/irq.h>
 
 extern struct kobject *vfiec_kobj;
 
@@ -37,18 +44,23 @@ enum gsensor_orientation_hex {
 #define ST_ACCEL_CTRL_REG1_ADDR     0x20
 #define ST_ACCEL_CTRL_REG4_ADDR     0x23
 #define ST_ACCEL_OUT_X_L_ADDR       0x28
+#define ST_ACCEL_STATUS_REG_ADDR    0x27
 
 #define SC7A20_WHO_AM_I_VALUE       0x11
 
 /* Configuration */
 #define GSENSOR_DEBOUNCE_MS     300
 #define GSENSOR_POLL_INTERVAL_MS 200
+#define ST_ACCEL_NUMBER_DATA_CHANNELS 3
+
+/* Driver Data Structure */
 
 struct gsensor_data {
     struct i2c_client *client;
     struct mutex lock;
     struct delayed_work poll_work;
     struct kobject *gsensor_kobj;
+    struct iio_dev *indio_dev;
     
     bool enabled;
     bool initialized;
@@ -58,7 +70,11 @@ struct gsensor_data {
     char orientation_str[32];
     unsigned long last_change_jiffies;
     
+    /* IIO buffer data */
+    s16 buffer[ST_ACCEL_NUMBER_DATA_CHANNELS + 1];
+    
     u8 who_am_i;
+    int irq;
 };
 
 static struct gsensor_data *g_data = NULL;
@@ -78,8 +94,8 @@ static int gsensor_write_reg(struct i2c_client *client, u8 reg, u8 value)
     return i2c_smbus_write_byte_data(client, reg, value);
 }
 
-/* Read the raw data */
-static int gsensor_read_data(struct gsensor_data *data, int *x, int *y, int *z)
+/* Read raw acceleration data (12-bit, right-aligned) */
+static int gsensor_read_raw_data(struct gsensor_data *data, int *x, int *y, int *z)
 {
     u8 xl, xh, yl, yh, zl, zh;
     s16 raw_x, raw_y, raw_z;
@@ -136,8 +152,14 @@ static int gsensor_set_enable(struct gsensor_data *data, bool enable)
     return 0;
 }
 
-/* Initialize sensor */
-static int gsensor_init(struct gsensor_data *data)
+static int gsensor_set_dataready_irq(struct gsensor_data *data, bool enable)
+{
+    /* For SC7A20, data ready is automatically enabled when sensor is active */
+    /* This function is for compatibility with st_* trigger framework */
+    return 0;
+}
+
+static int gsensor_init_sensor(struct gsensor_data *data)
 {
     u8 who_am_i;
     int ret;
@@ -157,7 +179,7 @@ static int gsensor_init(struct gsensor_data *data)
         return -ENODEV;
     }
 
-    /* 2. Setting CTRL_REG4: BDU enable , ±2g */
+    /* 2. Setting CTRL_REG4: BDU enable */
     ret = gsensor_write_reg(data->client, ST_ACCEL_CTRL_REG4_ADDR, 0x80);
     if (ret < 0)
         return ret;
@@ -213,7 +235,7 @@ static void gsensor_update_orientation(struct gsensor_data *data)
     if (!data->enabled)
         return;
     
-    if (gsensor_read_data(data, &x, &y, &z) < 0)
+    if (gsensor_read_raw_data(data, &x, &y, &z) < 0)
         return;
     
     new_orient = gsensor_calc_orientation(x, y, z);
@@ -268,7 +290,212 @@ static void gsensor_poll_work(struct work_struct *work)
                               msecs_to_jiffies(GSENSOR_POLL_INTERVAL_MS));
 }
 
-/* Sysfs Interface */
+/* IIO Framework */
+
+static int gsensor_read_raw(struct iio_dev *indio_dev,
+                            struct iio_chan_spec const *ch,
+                            int *val, int *val2, long mask)
+{
+    struct gsensor_data *data = iio_priv(indio_dev);
+    int x, y, z;
+    int err;
+
+    switch (mask) {
+    case IIO_CHAN_INFO_RAW:
+        err = gsensor_read_raw_data(data, &x, &y, &z);
+        if (err < 0)
+            return err;
+
+        switch (ch->channel2) {
+        case IIO_MOD_X:
+            *val = x;
+            return IIO_VAL_INT;
+        case IIO_MOD_Y:
+            *val = y;
+            return IIO_VAL_INT;
+        case IIO_MOD_Z:
+            *val = z;
+            return IIO_VAL_INT;
+        default:
+            return -EINVAL;
+        }
+        
+    case IIO_CHAN_INFO_SCALE:
+        *val = 0;
+        *val2 = 61035;  /* 0.000061035 g */
+        return IIO_VAL_INT_PLUS_NANO;
+        
+    case IIO_CHAN_INFO_SAMP_FREQ:
+        *val = 100;
+        return IIO_VAL_INT;
+        
+    default:
+        return -EINVAL;
+    }
+}
+
+static int gsensor_write_raw(struct iio_dev *indio_dev,
+                             struct iio_chan_spec const *ch,
+                             int val, int val2, long mask)
+{
+    switch (mask) {
+    case IIO_CHAN_INFO_SAMP_FREQ:
+        if (val == 100)
+            return 0;
+        return -EINVAL;
+    default:
+        return -EINVAL;
+    }
+}
+
+/* IIO channels */
+static const struct iio_chan_spec gsensor_channels[] = {
+    {
+        .type = IIO_ACCEL,
+        .modified = 1,
+        .channel2 = IIO_MOD_X,
+        .info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+        .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) |
+                                    BIT(IIO_CHAN_INFO_SAMP_FREQ),
+        .scan_index = 0,
+        .scan_type = {
+            .sign = 's',
+            .realbits = 16,
+            .storagebits = 16,
+            .endianness = IIO_LE,
+        },
+    },
+    {
+        .type = IIO_ACCEL,
+        .modified = 1,
+        .channel2 = IIO_MOD_Y,
+        .info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+        .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) |
+                                    BIT(IIO_CHAN_INFO_SAMP_FREQ),
+        .scan_index = 1,
+        .scan_type = {
+            .sign = 's',
+            .realbits = 16,
+            .storagebits = 16,
+            .endianness = IIO_LE,
+        },
+    },
+    {
+        .type = IIO_ACCEL,
+        .modified = 1,
+        .channel2 = IIO_MOD_Z,
+        .info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+        .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) |
+                                    BIT(IIO_CHAN_INFO_SAMP_FREQ),
+        .scan_index = 2,
+        .scan_type = {
+            .sign = 's',
+            .realbits = 16,
+            .storagebits = 16,
+            .endianness = IIO_LE,
+        },
+    },
+    IIO_CHAN_SOFT_TIMESTAMP(3),
+};
+
+/* Trigger functions */
+static int gsensor_trig_set_state(struct iio_trigger *trig, bool state)
+{
+    struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
+    struct gsensor_data *data = iio_priv(indio_dev);
+    
+    return gsensor_set_dataready_irq(data, state);
+}
+
+static const struct iio_trigger_ops gsensor_trigger_ops = {
+    .set_trigger_state = gsensor_trig_set_state,
+    .validate_device = iio_trigger_validate_own_device,
+};
+
+/* Buffer functions */
+static int gsensor_buffer_postenable(struct iio_dev *indio_dev)
+{
+    struct gsensor_data *data = iio_priv(indio_dev);
+    int err;
+
+    err = gsensor_set_enable(data, true);
+    if (err < 0)
+        return err;
+
+    return gsensor_set_dataready_irq(data, true);
+}
+
+static int gsensor_buffer_predisable(struct iio_dev *indio_dev)
+{
+    struct gsensor_data *data = iio_priv(indio_dev);
+    int err;
+
+    err = gsensor_set_dataready_irq(data, false);
+    if (err < 0)
+        return err;
+
+    return gsensor_set_enable(data, false);
+}
+
+static const struct iio_buffer_setup_ops gsensor_buffer_setup_ops = {
+    .postenable = &gsensor_buffer_postenable,
+    .predisable = &gsensor_buffer_predisable,
+};
+
+/* Trigger handler */
+static irqreturn_t gsensor_trigger_handler(int irq, void *p)
+{
+    struct iio_poll_func *pf = p;
+    struct iio_dev *indio_dev = pf->indio_dev;
+    struct gsensor_data *data = iio_priv(indio_dev);
+    int x, y, z;
+
+    gsensor_read_raw_data(data, &x, &y, &z);
+    data->buffer[0] = x;
+    data->buffer[1] = y;
+    data->buffer[2] = z;
+
+    iio_push_to_buffers_with_timestamp(indio_dev, data->buffer,
+                                       iio_get_time_ns(indio_dev));
+    iio_trigger_notify_done(indio_dev->trig);
+
+    return IRQ_HANDLED;
+}
+
+/* IIO attributes */
+static ssize_t gsensor_show_available_freq(struct device *dev,
+                                           struct device_attribute *attr,
+                                           char *buf)
+{
+    return sprintf(buf, "100\n");
+}
+static IIO_DEVICE_ATTR(sampling_frequency_available, 0444,
+                       gsensor_show_available_freq, NULL, 0);
+
+static struct attribute *gsensor_iio_attrs[] = {
+    &iio_dev_attr_sampling_frequency_available.dev_attr.attr,
+    NULL,
+};
+
+static const struct attribute_group gsensor_iio_attr_group = {
+    .attrs = gsensor_iio_attrs,
+};
+
+static const struct iio_info gsensor_iio_info = {
+    .read_raw = gsensor_read_raw,
+    .write_raw = gsensor_write_raw,
+    .attrs = &gsensor_iio_attr_group,
+};
+
+/* Allocate ring buffer and trigger */
+static int gsensor_allocate_ring(struct iio_dev *indio_dev)
+{
+    return devm_iio_triggered_buffer_setup(indio_dev->dev.parent, indio_dev,
+        NULL, &gsensor_trigger_handler, &gsensor_buffer_setup_ops);
+}
+
+/* ==================== Verifone Sysfs Interface ==================== */
+
 static ssize_t enable_show(struct kobject *kobj, struct kobj_attribute *attr, char *buf)
 {
     return sprintf(buf, "%d\n", g_data ? g_data->enabled : 0);
@@ -345,7 +572,7 @@ static ssize_t mode_store(struct kobject *kobj, struct kobj_attribute *attr,
     
     if (val & GSENSOR_MODE_SOFT_RESET) {
         mutex_lock(&data->lock);
-        gsensor_init(data);
+        gsensor_init_sensor(data);
         data->orientation_hex = GSENSOR_ORIENT_LANDSCAPE_HEX;
         strcpy(data->orientation_str, GSENSOR_ORIENT_LANDSCAPE_STR);
         mutex_unlock(&data->lock);
@@ -371,7 +598,7 @@ static ssize_t raw_data_show(struct kobject *kobj, struct kobj_attribute *attr,
     if (!g_data)
         return -ENODEV;
     
-    if (gsensor_read_data(g_data, &x, &y, &z) < 0)
+    if (gsensor_read_raw_data(g_data, &x, &y, &z) < 0)
         return -EIO;
     
     return sprintf(buf, "%d %d %d\n", x, y, z);
@@ -395,33 +622,73 @@ static const struct attribute_group gsensor_attr_group = {
 /* I2C Probe/Remove */
 static int gsensor_probe(struct i2c_client *client)
 {
+    struct iio_dev *indio_dev;
     struct gsensor_data *data;
     int ret;
-    
-    dev_info(&client->dev, "Gsensor probe\n");
-    
-    data = kzalloc(sizeof(*data), GFP_KERNEL);
-    if (!data)
+
+    indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+    if (!indio_dev)
         return -ENOMEM;
-    
-    data->client = client;
+
+    data            = iio_priv(indio_dev);
+    data->client    = client;
+    data->indio_dev = indio_dev;
+    data->irq       = client->irq;
+
     mutex_init(&data->lock);
     INIT_DELAYED_WORK(&data->poll_work, gsensor_poll_work);
     data->orientation_hex = GSENSOR_ORIENT_LANDSCAPE_HEX;
     strcpy(data->orientation_str, GSENSOR_ORIENT_LANDSCAPE_STR);
     data->pending_orientation = GSENSOR_ORIENT_UNKNOWN_HEX;
     data->last_change_jiffies = jiffies;
-    
-    i2c_set_clientdata(client, data);
-    
-    ret = gsensor_init(data);
+
+    i2c_set_clientdata(client, indio_dev);
+
+    ret = gsensor_init_sensor(data);
     if (ret < 0) {
         dev_err(&client->dev, "Init failed: %d\n", ret);
-        goto err_free;
+        return ret;
     }
-    
+
+    indio_dev->modes        = INDIO_DIRECT_MODE;
+    indio_dev->info         = &gsensor_iio_info;
+    indio_dev->channels     = gsensor_channels;
+    indio_dev->num_channels = ARRAY_SIZE(gsensor_channels);
+
+    ret = gsensor_allocate_ring(indio_dev);
+    if (ret < 0) {
+        dev_err(&client->dev, "Failed to allocate ring buffer: %d\n", ret);
+        return ret;
+    }
+
+    if (data->irq > 0) {
+        struct iio_trigger *trig;
+
+        trig = devm_iio_trigger_alloc(&client->dev, "%s-dev%d", indio_dev->name,
+                                      iio_device_id(indio_dev));
+        if (!trig) {
+            ret = -ENOMEM;
+            goto error;
+        }
+
+        trig->ops = &gsensor_trigger_ops;
+        iio_trigger_set_drvdata(trig, indio_dev);
+
+        ret = devm_iio_trigger_register(&client->dev, trig);
+        if (ret < 0)
+            goto error;
+
+        indio_dev->trig = iio_trigger_get(trig);
+    }
+
+    ret = iio_device_register(indio_dev);
+    if (ret < 0) {
+        dev_err(&client->dev, "Failed to register IIO device: %d\n", ret);
+        goto error;
+    }
+
     g_data = data;
-    
+
     if (vfiec_kobj) {
         data->gsensor_kobj = kobject_create_and_add("gsensor", vfiec_kobj);
         if (data->gsensor_kobj) {
@@ -429,29 +696,43 @@ static int gsensor_probe(struct i2c_client *client)
             if (ret < 0) {
                 dev_err(&client->dev, "Failed to create sysfs group\n");
                 kobject_put(data->gsensor_kobj);
-                goto err_free;
+                goto error_unregister;
             }
         } else {
             dev_err(&client->dev, "Failed to create gsensor kobject\n");
-            goto err_free;
+            goto error_unregister;
         }
     } else {
         dev_err(&client->dev, "vfiec_kobj not available\n");
-        goto err_free;
+        goto error_unregister;
     }
-    
-    dev_info(&client->dev, "Gsensor driver loaded\n");
+
+    /* ========== auto enable the sensor========== */
+    ret = gsensor_set_enable(data, true);
+    if (ret == 0) {
+        data->enabled = true;
+        schedule_delayed_work(&data->poll_work, 0);
+        gsensor_update_orientation(data);
+        dev_info(&client->dev, "Gsensor auto-enabled on load\n");
+    } else {
+        dev_warn(&client->dev, "Failed to auto-enable sensor: %d\n", ret);
+    }
+    /* ================================= */
+
+    dev_info(&client->dev, "Gsensor driver loaded (WHO_AM_I=0x%02x)\n",
+             data->who_am_i);
     return 0;
 
-err_free:
-    kfree(data);
-    g_data = NULL;
+error_unregister:
+    iio_device_unregister(indio_dev);
+error:
     return ret;
 }
 
 static int gsensor_remove(struct i2c_client *client)
 {
-    struct gsensor_data *data = i2c_get_clientdata(client);
+    struct iio_dev *indio_dev = i2c_get_clientdata(client);
+    struct gsensor_data *data = iio_priv(indio_dev);
     
     if (data) {
         cancel_delayed_work_sync(&data->poll_work);
@@ -461,14 +742,15 @@ static int gsensor_remove(struct i2c_client *client)
             sysfs_remove_group(data->gsensor_kobj, &gsensor_attr_group);
             kobject_put(data->gsensor_kobj);
         }
+        iio_device_unregister(indio_dev);
         mutex_destroy(&data->lock);
-        kfree(data);
         g_data = NULL;
     }
     return 0;
 }
 
-/* I2C Driver */
+/* ==================== I2C Driver ==================== */
+
 static const struct i2c_device_id gsensor_id_table[] = {
     { "sc7a20", 0 },
     { "gsensor", 0 },
@@ -477,7 +759,9 @@ static const struct i2c_device_id gsensor_id_table[] = {
 MODULE_DEVICE_TABLE(i2c, gsensor_id_table);
 
 static struct i2c_driver gsensor_driver = {
-    .driver = { .name = "verifone-gsensor" },
+    .driver = {
+        .name = "verifone-gsensor",
+    },
     .probe_new = gsensor_probe,
     .remove = gsensor_remove,
     .id_table = gsensor_id_table,
@@ -501,3 +785,4 @@ EXPORT_SYMBOL(gsensor_exit_main);
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Verifone, Inc.");
 MODULE_DESCRIPTION("Verifone Gsensor driver for screen orientation");
+