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

/* Driver Data Structure */
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;
    int irq;
};

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 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;
    int ret;

    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;

    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;

    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) {
        ret = gsensor_write_reg(data->client, ST_ACCEL_CTRL_REG1_ADDR, 0x87);
        if (ret < 0)
            return ret;
        msleep(20);
    } else {
        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_sensor(struct gsensor_data *data)
{
    u8 who_am_i;
    int ret;

    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;
    }

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

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

    if (abs_z > 800 && abs_z > abs_x && abs_z > abs_y) {
        return GSENSOR_ORIENT_UNKNOWN_HEX;
    }

    if (abs_x > 800 && abs_x > abs_y) {
        if (x > 0)
            return GSENSOR_ORIENT_LANDSCAPE_FLIP_HEX;
        else
            return GSENSOR_ORIENT_LANDSCAPE_HEX;
    }

    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_raw_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));
}

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

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_sensor(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_raw_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;
    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_sensor(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;
    }

    /* 自动启用传感器 */
    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;
    return ret;
}

static int gsensor_remove(struct i2c_client *client)
{
    struct gsensor_data *data = g_data;

    if (data) {
        dev_info(&client->dev, "Removing Gsensor driver...\n");
        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 ==================== */
static void gsensor_auto_create_i2c_device(void)
{
    struct i2c_adapter *adapter;
    struct i2c_board_info info = {
        I2C_BOARD_INFO("sc7a20", 0x19),
    };
    
    printk(KERN_INFO "gsensor: auto_create_i2c_device START\n");
    
    adapter = i2c_get_adapter(0);
    if (adapter) {
        printk(KERN_INFO "gsensor: got adapter for bus 0\n");
        i2c_new_client_device(adapter, &info);
        i2c_put_adapter(adapter);
        printk(KERN_INFO "gsensor: created device at 0x19 on bus 0\n");
    } else {
        printk(KERN_INFO "gsensor: no adapter for bus 0\n");
    }
    
    printk(KERN_INFO "gsensor: auto_create_i2c_device END\n");
}

int gsensor_init_main(void)
{
    int ret;
    
    printk(KERN_INFO "gsensor: Registering driver START\n");
    
    ret = i2c_add_driver(&gsensor_driver);
    if (ret < 0) {
        printk(KERN_ERR "gsensor: Failed to register I2C driver: %d\n", ret);
        return ret;
    }
    
    printk(KERN_INFO "gsensor: i2c_add_driver done\n");
    
    gsensor_auto_create_i2c_device();
    
    printk(KERN_INFO "gsensor: Driver initialized\n");
    return 0;
}

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");