C180_oyster_driver/gsensor.c

/* SPDX-License-Identifier: GPL-2.0-only
 *
 * Verifone Gsensor driver for screen orientation
 */

#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/init.h>
#include <linux/i2c.h>
#include <linux/delay.h>
#include <linux/mutex.h>
#include <linux/sysfs.h>
#include <linux/slab.h>
#include <linux/workqueue.h>

extern struct kobject *vfiec_kobj;

/* Verifone API Definitions */
enum gsensor_orientation_hex {
    GSENSOR_ORIENT_UNKNOWN_HEX      = 0x00,
    GSENSOR_ORIENT_PORTRAIT_HEX     = 0x14,
    GSENSOR_ORIENT_LANDSCAPE_HEX    = 0x15,
    GSENSOR_ORIENT_PORTRAIT_FLIP_HEX = 0x16,
    GSENSOR_ORIENT_LANDSCAPE_FLIP_HEX = 0x17,
};

#define GSENSOR_ORIENT_PORTRAIT_STR     "portrait"
#define GSENSOR_ORIENT_LANDSCAPE_STR    "landscape"
#define GSENSOR_ORIENT_PORTRAIT_FLIP_STR "portrait_flip"
#define GSENSOR_ORIENT_LANDSCAPE_FLIP_STR "landscape_flip"

#define GSENSOR_MODE_SOFT_RESET     BIT(2)
#define GSENSOR_MODE_INITIALIZED    BIT(1)

/* Register Definitions */
#define ST_ACCEL_WHO_AM_I_ADDR      0x0F
#define ST_ACCEL_CTRL_REG1_ADDR     0x20
#define ST_ACCEL_CTRL_REG4_ADDR     0x23
#define ST_ACCEL_OUT_X_L_ADDR       0x28

#define SC7A20_WHO_AM_I_VALUE       0x11

/* Configuration */
#define GSENSOR_DEBOUNCE_MS     300
#define GSENSOR_POLL_INTERVAL_MS 200

struct gsensor_data {
    struct i2c_client *client;
    struct mutex lock;
    struct delayed_work poll_work;
    struct kobject *gsensor_kobj;
    
    bool enabled;
    bool initialized;
    
    enum gsensor_orientation_hex orientation_hex;
    enum gsensor_orientation_hex pending_orientation;
    char orientation_str[32];
    unsigned long last_change_jiffies;
    
    u8 who_am_i;
};

static struct gsensor_data *g_data = NULL;

/* I2C Helper Functions */
static int gsensor_read_reg(struct i2c_client *client, u8 reg, u8 *data)
{
    int ret = i2c_smbus_read_byte_data(client, reg);
    if (ret < 0)
        return ret;
    *data = ret;
    return 0;
}

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)
{
    u8 xl, xh, yl, yh, zl, zh;
    s16 raw_x, raw_y, raw_z;
    int ret;

    /* Read the X-axis byte by byte */
    ret = gsensor_read_reg(data->client, 0x28, &xl);
    if (ret < 0) return ret;
    ret = gsensor_read_reg(data->client, 0x29, &xh);
    if (ret < 0) return ret;

    /* Read the Y-axis byte by byte */
    ret = gsensor_read_reg(data->client, 0x2a, &yl);
    if (ret < 0) return ret;
    ret = gsensor_read_reg(data->client, 0x2b, &yh);
    if (ret < 0) return ret;

    /* Z-axis */
    ret = gsensor_read_reg(data->client, 0x2c, &zl);
    if (ret < 0) return ret;
    ret = gsensor_read_reg(data->client, 0x2d, &zh);
    if (ret < 0) return ret;
    
    raw_x = (s16)((xh << 8) | xl);
    raw_y = (s16)((yh << 8) | yl);
    raw_z = (s16)((zh << 8) | zl);

    /* ±2g Mode: 1 LSB = 0.06103515625 mg */
    *x = (raw_x * 61) / 1000;
    *y = (raw_y * 61) / 1000;
    *z = (raw_z * 61) / 1000;
    
    return 0;
}

/* Enable/Disable sensor */
static int gsensor_set_enable(struct gsensor_data *data, bool enable)
{
    int ret;
    
    if (enable) {
        /* CTRL_REG1: 0x87 = 100Hz (0x80) + Enable all axes (0x07) */
        ret = gsensor_write_reg(data->client, ST_ACCEL_CTRL_REG1_ADDR, 0x87);
        if (ret < 0)
            return ret;
        msleep(20);
    } else {
        /* CTRL_REG1: 0x00 = Standby mode */
        ret = gsensor_write_reg(data->client, ST_ACCEL_CTRL_REG1_ADDR, 0x00);
        if (ret < 0)
            return ret;
    }
    
    return 0;
}

/* Initialize sensor */
static int gsensor_init(struct gsensor_data *data)
{
    u8 who_am_i;
    int ret;

    /* 1. Verify device */
    ret = gsensor_read_reg(data->client, ST_ACCEL_WHO_AM_I_ADDR, &who_am_i);
    if (ret < 0) {
        dev_err(&data->client->dev, "Failed to read WHO_AM_I: %d\n", ret);
        return ret;
    }
    
    data->who_am_i = who_am_i;
    dev_info(&data->client->dev, "WHO_AM_I = 0x%02x\n", who_am_i);
    
    if (who_am_i != SC7A20_WHO_AM_I_VALUE) {
        dev_err(&data->client->dev, "Invalid WHO_AM_I: 0x%02x\n", who_am_i);
        return -ENODEV;
    }

    /* 2. Setting CTRL_REG4: BDU enable , ±2g */
    ret = gsensor_write_reg(data->client, ST_ACCEL_CTRL_REG4_ADDR, 0x80);
    if (ret < 0)
        return ret;

    /* 3. Disable the sensor at startup */
    ret = gsensor_write_reg(data->client, ST_ACCEL_CTRL_REG1_ADDR, 0x00);
    if (ret < 0)
        return ret;
    
    data->initialized = true;
    dev_info(&data->client->dev, "Gsensor initialized successfully\n");
    
    return 0;
}

/* Orientation Calculation */
static enum gsensor_orientation_hex gsensor_calc_orientation(int x, int y, int z)
{
    int abs_x = abs(x);
    int abs_y = abs(y);
    int abs_z = abs(z);

    /* Device flat - Z axis maximum */
    if (abs_z > 800 && abs_z > abs_x && abs_z > abs_y) {
        return GSENSOR_ORIENT_UNKNOWN_HEX;
    }

    /* Landscape mode: X axis maximum */
    if (abs_x > 800 && abs_x > abs_y) {
        if (x > 0)
            return GSENSOR_ORIENT_LANDSCAPE_FLIP_HEX;
        else
            return GSENSOR_ORIENT_LANDSCAPE_HEX;
    }

    /* Portrait mode: Y axis maximum. */
    if (abs_y > 800 && abs_y > abs_x) {
        if (y > 0)
            return GSENSOR_ORIENT_PORTRAIT_FLIP_HEX;
        else
            return GSENSOR_ORIENT_PORTRAIT_HEX;
    }
    
    return GSENSOR_ORIENT_UNKNOWN_HEX;
}

static void gsensor_update_orientation(struct gsensor_data *data)
{
    int x, y, z;
    enum gsensor_orientation_hex new_orient;
    unsigned long now = jiffies;
    
    if (!data->enabled)
        return;
    
    if (gsensor_read_data(data, &x, &y, &z) < 0)
        return;
    
    new_orient = gsensor_calc_orientation(x, y, z);
    
    if (new_orient != GSENSOR_ORIENT_UNKNOWN_HEX && new_orient != data->orientation_hex) {
        if (new_orient == data->pending_orientation) {
            if (time_after(now, data->last_change_jiffies + 
                           msecs_to_jiffies(GSENSOR_DEBOUNCE_MS))) {
                data->orientation_hex = new_orient;
                data->pending_orientation = GSENSOR_ORIENT_UNKNOWN_HEX;
                
                switch (new_orient) {
                case GSENSOR_ORIENT_PORTRAIT_HEX:
                    strcpy(data->orientation_str, GSENSOR_ORIENT_PORTRAIT_STR);
                    break;
                case GSENSOR_ORIENT_LANDSCAPE_HEX:
                    strcpy(data->orientation_str, GSENSOR_ORIENT_LANDSCAPE_STR);
                    break;
                case GSENSOR_ORIENT_PORTRAIT_FLIP_HEX:
                    strcpy(data->orientation_str, GSENSOR_ORIENT_PORTRAIT_FLIP_STR);
                    break;
                case GSENSOR_ORIENT_LANDSCAPE_FLIP_HEX:
                    strcpy(data->orientation_str, GSENSOR_ORIENT_LANDSCAPE_FLIP_STR);
                    break;
                default:
                    break;
                }
                
                dev_info(&data->client->dev, "Orientation: %s [X=%d, Y=%d, Z=%d]\n",
                         data->orientation_str, x, y, z);
            }
        } else {
            data->pending_orientation = new_orient;
            data->last_change_jiffies = now;
        }
    } else if (new_orient == data->orientation_hex) {
        data->pending_orientation = GSENSOR_ORIENT_UNKNOWN_HEX;
    }
}

static void gsensor_poll_work(struct work_struct *work)
{
    struct gsensor_data *data = container_of(work, struct gsensor_data,
                                             poll_work.work);
    
    mutex_lock(&data->lock);
    gsensor_update_orientation(data);
    mutex_unlock(&data->lock);
    
    if (data->enabled)
        schedule_delayed_work(&data->poll_work, 
                              msecs_to_jiffies(GSENSOR_POLL_INTERVAL_MS));
}

/* 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);
}

static ssize_t enable_store(struct kobject *kobj, struct kobj_attribute *attr,
                            const char *buf, size_t count)
{
    struct gsensor_data *data = g_data;
    unsigned long val;
    int ret;
    
    if (!data)
        return -ENODEV;
    
    ret = kstrtoul(buf, 0, &val);
    if (ret)
        return ret;
    
    mutex_lock(&data->lock);
    
    if (val && !data->enabled) {
        ret = gsensor_set_enable(data, true);
        if (ret == 0) {
            data->enabled = true;
            schedule_delayed_work(&data->poll_work, 0);
            gsensor_update_orientation(data);
        }
    } else if (!val && data->enabled) {
        cancel_delayed_work_sync(&data->poll_work);
        ret = gsensor_set_enable(data, false);
        if (ret == 0)
            data->enabled = false;
    }
    
    mutex_unlock(&data->lock);
    return ret < 0 ? ret : count;
}
static struct kobj_attribute enable_attr = __ATTR_RW(enable);

static ssize_t screen_orientation_show(struct kobject *kobj,
                                       struct kobj_attribute *attr, char *buf)
{
    return sprintf(buf, "%s\n", g_data ? g_data->orientation_str : "unknown");
}
static struct kobj_attribute screen_orientation_attr = __ATTR_RO(screen_orientation);

static ssize_t instantaneous_orientation_show(struct kobject *kobj,
                                              struct kobj_attribute *attr, char *buf)
{
    return sprintf(buf, "0x%02x\n", g_data ? g_data->orientation_hex : 0);
}
static struct kobj_attribute instantaneous_orientation_attr = __ATTR_RO(instantaneous_orientation);

static ssize_t mode_show(struct kobject *kobj, struct kobj_attribute *attr, char *buf)
{
    u8 mode = 0;
    if (g_data && g_data->initialized)
        mode |= GSENSOR_MODE_INITIALIZED;
    return sprintf(buf, "0x%02x\n", mode);
}

static ssize_t mode_store(struct kobject *kobj, struct kobj_attribute *attr,
                          const char *buf, size_t count)
{
    struct gsensor_data *data = g_data;
    unsigned long val;
    
    if (!data)
        return -ENODEV;
    
    if (kstrtoul(buf, 0, &val))
        return -EINVAL;
    
    if (val & GSENSOR_MODE_SOFT_RESET) {
        mutex_lock(&data->lock);
        gsensor_init(data);
        data->orientation_hex = GSENSOR_ORIENT_LANDSCAPE_HEX;
        strcpy(data->orientation_str, GSENSOR_ORIENT_LANDSCAPE_STR);
        mutex_unlock(&data->lock);
    }
    
    return count;
}
static struct kobj_attribute mode_attr = __ATTR_RW(mode);

static ssize_t state_show(struct kobject *kobj, struct kobj_attribute *attr, char *buf)
{
    u8 state = 0;
    if (g_data && g_data->enabled)
        state |= 0x01;
    return sprintf(buf, "0x%02x\n", state);
}
static struct kobj_attribute state_attr = __ATTR_RO(state);

static ssize_t raw_data_show(struct kobject *kobj, struct kobj_attribute *attr, char *buf)
{
    int x, y, z;
    
    if (!g_data)
        return -ENODEV;
    
    if (gsensor_read_data(g_data, &x, &y, &z) < 0)
        return -EIO;
    
    return sprintf(buf, "%d %d %d\n", x, y, z);
}
static struct kobj_attribute raw_data_attr = __ATTR_RO(raw_data);

static struct attribute *gsensor_attrs[] = {
    &enable_attr.attr,
    &screen_orientation_attr.attr,
    &instantaneous_orientation_attr.attr,
    &mode_attr.attr,
    &state_attr.attr,
    &raw_data_attr.attr,
    NULL,
};

static const struct attribute_group gsensor_attr_group = {
    .attrs = gsensor_attrs,
};

/* I2C Probe/Remove */
static int gsensor_probe(struct i2c_client *client)
{
    struct gsensor_data *data;
    int ret;
    
    dev_info(&client->dev, "Gsensor probe\n");
    
    data = kzalloc(sizeof(*data), GFP_KERNEL);
    if (!data)
        return -ENOMEM;
    
    data->client = client;
    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);
    if (ret < 0) {
        dev_err(&client->dev, "Init failed: %d\n", ret);
        goto err_free;
    }
    
    g_data = data;
    
    if (vfiec_kobj) {
        data->gsensor_kobj = kobject_create_and_add("gsensor", vfiec_kobj);
        if (data->gsensor_kobj) {
            ret = sysfs_create_group(data->gsensor_kobj, &gsensor_attr_group);
            if (ret < 0) {
                dev_err(&client->dev, "Failed to create sysfs group\n");
                kobject_put(data->gsensor_kobj);
                goto err_free;
            }
        } else {
            dev_err(&client->dev, "Failed to create gsensor kobject\n");
            goto err_free;
        }
    } else {
        dev_err(&client->dev, "vfiec_kobj not available\n");
        goto err_free;
    }
    
    dev_info(&client->dev, "Gsensor driver loaded\n");
    return 0;

err_free:
    kfree(data);
    g_data = NULL;
    return ret;
}

static int gsensor_remove(struct i2c_client *client)
{
    struct gsensor_data *data = i2c_get_clientdata(client);
    
    if (data) {
        cancel_delayed_work_sync(&data->poll_work);
        if (data->enabled)
            gsensor_set_enable(data, false);
        if (data->gsensor_kobj) {
            sysfs_remove_group(data->gsensor_kobj, &gsensor_attr_group);
            kobject_put(data->gsensor_kobj);
        }
        mutex_destroy(&data->lock);
        kfree(data);
        g_data = NULL;
    }
    return 0;
}

/* I2C Driver */
static const struct i2c_device_id gsensor_id_table[] = {
    { "sc7a20", 0 },
    { "gsensor", 0 },
    {}
};
MODULE_DEVICE_TABLE(i2c, gsensor_id_table);

static struct i2c_driver gsensor_driver = {
    .driver = { .name = "verifone-gsensor" },
    .probe_new = gsensor_probe,
    .remove = gsensor_remove,
    .id_table = gsensor_id_table,
};

/* Init/Exit */
int gsensor_init_main(void)
{
    printk(KERN_INFO "gsensor: Registering driver\n");
    return i2c_add_driver(&gsensor_driver);
}
EXPORT_SYMBOL(gsensor_init_main);

void gsensor_exit_main(void)
{
    printk(KERN_INFO "gsensor: Unregistering driver\n");
    i2c_del_driver(&gsensor_driver);
}
EXPORT_SYMBOL(gsensor_exit_main);

MODULE_LICENSE("GPL v2");
MODULE_AUTHOR("Verifone, Inc.");
MODULE_DESCRIPTION("Verifone Gsensor driver for screen orientation");