--- zzzz-none-000/linux-4.9.279/drivers/hid/hid-sensor-hub.c 2021-08-08 06:38:54.000000000 +0000 +++ puma7-atom-6591-750/linux-4.9.279/drivers/hid/hid-sensor-hub.c 2023-02-08 11:43:42.000000000 +0000 @@ -21,12 +21,42 @@ #include #include #include -#include #include #include #include +#include #include "hid-ids.h" +/*SENSCOL*/ +#include +/*IIO*/ +#include + +/*#define CONFIG_ISS_PATH_SENSCOL*/ + +/***************************************/ +#if 1 + +#ifdef CONFIG_ISS_PATH_SENSCOL +#define SENSCOL 1 +#else +#define SENSCOL 0 +#endif + +#ifdef CONFIG_ISS_PATH_IIO +#define IIO 1 +#else +#define IIO 0 +#endif + +#else + +#define SENSCOL 1 +#define IIO 0 + +#endif +/***************************************/ + #define HID_SENSOR_HUB_ENUM_QUIRK 0x01 /** @@ -49,8 +79,13 @@ int hid_sensor_client_cnt; unsigned long quirks; int ref_cnt; + int sensor_hub_index; /* Needed to identify sensor in a collection */ }; +#define MAX_HID_SENSOR_HUBS 32 +static struct hid_device *hid_sensor_hubs[MAX_HID_SENSOR_HUBS]; +static int sensor_hub_count; + /** * struct hid_sensor_hub_callbacks_list - Stores callback list * @list: list head. @@ -424,6 +459,9 @@ } + if (info->units == 0) + info->units = HID_USAGE_SENSOR_UNITS_MILLISECOND; + return ret; } EXPORT_SYMBOL_GPL(sensor_hub_input_get_attribute_info); @@ -471,6 +509,318 @@ } #endif +static bool is_supported(int physical) +{ + if (physical == 0x200073) + return true; + if (physical == 0x200041) + return true; + if (physical == 0x200076) + return true; + if (physical == 0x200083) + return true; + + return false; +} + +/****************************** SENSCOL block: START ******************************/ + +static int senscol_impl_added; +static int is_sens_data_field(unsigned usage); +static int get_field_index(struct hid_device *hdev, unsigned report_id, + unsigned usage, int report_idx); + +/* Get sensor's property by name */ +static struct sens_property *get_prop_by_name(struct sensor_def *sensor, + char *name) +{ + int i; + + for (i = 0; i < sensor->num_properties; ++i) + if (!strcmp(sensor->properties[i].name, name)) + return &sensor->properties[i]; + + return NULL; +} + +/* Get sensor's data field by name */ +static struct data_field *get_data_field_by_name(struct sensor_def *sensor, + char *name) +{ + int i; + + for (i = 0; i < sensor->num_data_fields; ++i) + if (!strcmp(sensor->data_fields[i].name, name)) + return &sensor->data_fields[i]; + + return NULL; +} + +static int get_field_index(struct hid_device *hdev, unsigned report_id, + unsigned usage, int report_type) +{ + int i = 0; + struct hid_report *report; + + report = sensor_hub_report(report_id, hdev, report_type /*HID_FEATURE_REPORT or HID_INPUT_REPORT*/); + if (!report) { + return -1; + } + + for (i = 0; i < report->maxfield; ++i) + if (report->field[i]->usage->hid == usage) + return i; + + return -1; +} + +/* + * The reason for this _ex() function is broken semantics and existing usage of sensor_hub_get_feature() that + * doesn't allow anything with ->report_count > 1 to be delivered. + * If that was fixed, existing callers would immediately buffer-overflow if such feature was delivered + * NOTES: + * - if ret != 0, contents of pvalue and count are undefined. + * - upon success, count is in s32 values (not in bytes) + */ +static int sensor_hub_get_feature_ex(struct hid_sensor_hub_device *hsdev, u32 report_id, u32 field_index, u32 *usage_id, s32 **pvalue, size_t *count) +{ + struct hid_report *report; + struct sensor_hub_data *data = hid_get_drvdata(hsdev->hdev); + int ret = 0; + + mutex_lock(&data->mutex); + report = sensor_hub_report(report_id, hsdev->hdev, HID_FEATURE_REPORT); + if (!report || (field_index >= report->maxfield) || + report->field[field_index]->report_count < 1) { + ret = -EINVAL; + goto done_proc; + } + hid_hw_request(hsdev->hdev, report, HID_REQ_GET_REPORT); + hid_hw_wait(hsdev->hdev); + *pvalue = report->field[field_index]->value; + *count = report->field[field_index]->report_count; + *usage_id = report->field[field_index]->usage->hid; + +done_proc: + mutex_unlock(&data->mutex); + + return ret; +} + +static bool hid_is_string_property(uint32_t usage_id) +{ + return ((usage_id == HID_USAGE_SENSOR_PROPERTY_SENSOR_DESCRIPTION) || (usage_id == HID_USAGE_SENSOR_PROPERTY_FRIENDLY_NAME)); +} + +/* Get sensor hub device by index */ +static struct sensor_hub_data *get_sensor_hub_by_index(unsigned idx) +{ + int i; + struct sensor_hub_data *sd; + struct hid_device *hdev; + + for (i = 0; i < sensor_hub_count; ++i) { + if (!hid_sensor_hubs[i]) + continue; + sd = hid_get_drvdata(hid_sensor_hubs[i]); + if (!sd) + continue; + if (sd->sensor_hub_index == idx) + return sd; + } + + return NULL; +} + +static int hid_get_sens_property(struct sensor_def *sensor, const struct sens_property* prop, char *value, size_t val_buf_size) +{ + unsigned idx; + struct sensor_hub_data *sd; + char buf[1024]; /* Enough for single property (?) */ + unsigned report_id; + int field; + uint32_t usage_id; + int32_t *pval; + size_t count; + int rv; + + if (!sensor || !prop) + return -EINVAL; /* input is invalid */ + + /* sensor hub device */ + idx = sensor->id >> 16 & 0xFFFF; + sd = get_sensor_hub_by_index(idx); + if (!sd) + return -EINVAL; /* sensor->id is bad */ + + /* Report ID */ + report_id = sensor->id & 0xFFFF; + + /* Field index */ + field = get_field_index(sd->hsdev->hdev, report_id, prop->usage_id, HID_FEATURE_REPORT); + if (field == -1) + return -EINVAL; /* Something is still wrong */ + + /* Get value */ + rv = sensor_hub_get_feature_ex(sd->hsdev, report_id, field, &usage_id, &pval, &count); + if (rv) + return rv; + + if (hid_is_string_property(usage_id)) { + int i; + + for (i = 0; i < count; ++i) + buf[i] = (char)pval[i]; + buf[i] = '\0'; + } else { + /* Verify output length */ + sprintf(buf, "%d", *pval); + } + + if (strlen(buf) >= val_buf_size) + return -EMSGSIZE; + strcpy(value, buf); + return 0; +} + +static int hid_set_sens_property(struct sensor_def *sensor, const struct sens_property *prop, const char *value) +{ + unsigned idx; + struct sensor_hub_data *sd; + unsigned report_id; + int field; + int32_t val; + int rv; + + if (!sensor || !prop) + return -EINVAL; /* input is invalid */ + + /* Value */ + rv = sscanf(value, " %d ", &val); + if (rv != 1) + return -EINVAL; /* Bad value */ + + /* sensor hub device */ + idx = sensor->id >> 16 & 0xFFFF; + sd = get_sensor_hub_by_index(idx); + if (!sd) + return -EINVAL; /* sensor->id is bad */ + + /* Report ID */ + report_id = sensor->id & 0xFFFF; + + /* Field index */ + field = get_field_index(sd->hsdev->hdev, report_id, prop->usage_id, HID_FEATURE_REPORT); + if (field == -1) + return -EINVAL; /* Something is still wrong */ + + /* Get value */ + rv = sensor_hub_set_feature(sd->hsdev, report_id, field, val); + return rv; +} + +static int hid_get_sample(struct sensor_def *sensor, void *sample_buf, size_t sample_buf_size) +{ + unsigned idx; + struct sensor_hub_data *sd; + unsigned report_id; + int field; + struct data_field *data_field; + int32_t val; + int rv; + + /* sensor hub device */ + idx = sensor->id >> 16 & 0xFFFF; + sd = get_sensor_hub_by_index(idx); + if (!sd) + return -EINVAL; /* sensor->id is bad */ + + /* Report ID */ + report_id = sensor->id & 0xFFFF; + + /* Request an input report with the first data field, regardless of what it is */ + data_field = &sensor->data_fields[0]; + val = sensor_hub_input_attr_get_raw_value(sd->hsdev, sensor->usage_id, data_field->usage_id, HID_INPUT_REPORT); + if (!sd->pending.status) + return -EIO; + + /* + * Actual sample will be pushed by sensor_hub_raw_event(). + * Invoke a short sleep in order to remove threads race condition and ensure that the sample is in senscol buffer + */ + schedule_timeout(2); + + return 0; +} + +/* Check sensor is activated and in batch mode * + * property_power_state = 2 hid_usage 0x200319 * + * property_reporting_state = 2/5 hid_usage 0x200316 * + * property_report_interval != 0 hid_usage 0x20030e * + * property_report_interval_resolution != 0 hid_usage 0x20530e * + * return value: 0 - sensor is not activated in batch * + * 1 - sensor is activated in batch */ +static int hid_batch_check(struct sensor_def *sensor) +{ + unsigned idx; + struct sensor_hub_data *sd; + unsigned report_id; + struct hid_report *report; + int field_idx; + __s32 val; + + idx = sensor->id >> 16 & 0xFFFF; + sd = get_sensor_hub_by_index(idx); + report_id = sensor->id & 0xFFFF; + report = sensor_hub_report(report_id, sd->hsdev->hdev, + HID_FEATURE_REPORT); + + /* property_power_state */ + field_idx = get_field_index(sd->hsdev->hdev, report_id, 0x200319, + HID_FEATURE_REPORT); + if (report->field[field_idx]->value[0] != 2) + return 0; + + /* property_reporting_state */ + field_idx = get_field_index(sd->hsdev->hdev, report_id, 0x200316, + HID_FEATURE_REPORT); + if (report->field[field_idx]->value[0] != 2 && + report->field[field_idx]->value[0] != 5) + return 0; + + /* property_report_interval */ + field_idx = get_field_index(sd->hsdev->hdev, report_id, 0x20030e, + HID_FEATURE_REPORT); + if (report->field[field_idx]->value[0] == 0) + return 0; + + /* property_report_interval_resolution */ + field_idx = get_field_index(sd->hsdev->hdev, report_id, 0x20530e, + HID_FEATURE_REPORT); + if (report->field[field_idx]->value[0] == 0) + return 0; + +dev_err(NULL, "%s() sensor 0x%x is in batch mode\n", __func__, sensor->id); + return 1; +} + +struct senscol_impl hid_senscol_impl = { + .get_sens_property = hid_get_sens_property, + .set_sens_property = hid_set_sens_property, + .get_sample = hid_get_sample, + .batch_check = hid_batch_check +}; + +static int is_sens_data_field(unsigned usage) +{ + if (usage >= 0x400 && usage <= 0x49F || usage >= 0x4B0 && usage <= 0x4DF || usage >= 0x4F0 && usage <= 0x4F7 || + usage >= 0x500 && usage <= 0x52F || usage >= 0x540 && usage <= 0x57F || usage >= 590 && usage <= 0x7FF) + return 1; + return 0; +} + +/******************************* SENSCOL block: END *******************************/ + /* * Handle raw report as sent by device */ @@ -482,16 +832,25 @@ int sz; struct sensor_hub_data *pdata = hid_get_drvdata(hdev); unsigned long flags; - struct hid_sensor_hub_callbacks *callback = NULL; struct hid_collection *collection = NULL; void *priv = NULL; struct hid_sensor_hub_device *hsdev = NULL; + /*#if SENSCOL*/ + uint32_t sensor_id; + unsigned char data_buf[1024]; + unsigned sample_size; +/*#endif*/ + +/*#if IIO*/ + struct hid_sensor_hub_callbacks *callback = NULL; +/*#endif*/ + hid_dbg(hdev, "sensor_hub_raw_event report id:0x%x size:%d type:%d\n", report->id, size, report->type); hid_dbg(hdev, "maxfield:%d\n", report->maxfield); if (report->type != HID_INPUT_REPORT) - return 1; + return 0; ptr = raw_data; if (report->id) @@ -499,6 +858,12 @@ spin_lock_irqsave(&pdata->lock, flags); +/*#if SENSCOL*/ + /* make up senscol id */ + sensor_id = pdata->sensor_hub_index << 16 | report->id & 0xFFFF; + sample_size = 0; +/*#endif*/ + for (i = 0; i < report->maxfield; ++i) { hid_dbg(hdev, "%d collection_index:%x hid:%x sz:%x\n", i, report->field[i]->usage->collection_index, @@ -507,6 +872,7 @@ report->field[i]->report_count)/8); sz = (report->field[i]->report_size * report->field[i]->report_count)/8; +/*#if IIO*/ collection = &hdev->collection[ report->field[i]->usage->collection_index]; hid_dbg(hdev, "collection->usage %x\n", @@ -542,13 +908,32 @@ report->field[i]->usage->hid, sz, ptr, callback->pdev); } +/*#endif*/ + +/*#if SENSCOL*/ + /* Prepare data for senscol sample */ + if (is_sens_data_field(report->field[i]->usage->hid & 0xFFFF)) { + dev_dbg(&hdev->dev, "%s(): aggregating, sz=%u \n", __func__, sample_size); + memcpy(data_buf + sample_size, ptr, sz); + sample_size += sz; + } +/*#endif*/ + /* If we want to add indication into raw stream that the last sample was synchronous, it's here: check for complete() condition above */ + ptr += sz; } +/*#if IIO*/ if (callback && collection && callback->send_event) callback->send_event(hsdev, collection->usage, callback->pdev); +/*endif*/ spin_unlock_irqrestore(&pdata->lock, flags); +/*#if SENSCOL*/ + /* Upstream sample to sensor collection framework */ + dev_dbg(&hdev->dev, "%s(): calling push_sample, aggregated sample size is %u\n", __func__, sample_size); + push_sample(sensor_id, data_buf); +/*#endif*/ return 1; } @@ -633,6 +1018,51 @@ return rdesc; } + +static int fill_data_field(struct hid_field *field, unsigned usage, + int need_internal_index, int k, struct sensor_def *senscol_sensor) +{ + struct data_field data_field; + char *usage_name; + int rv; + + memset(&data_field, 0, sizeof(struct data_field)); + usage_name = senscol_usage_to_name(usage & 0xFFFF); + if (usage_name) + data_field.name = need_internal_index ? + kasprintf(GFP_KERNEL, "%s_%d", usage_name, k) : + kasprintf(GFP_KERNEL, "%s", usage_name); + else { + data_field.name = + need_internal_index ? + kasprintf(GFP_KERNEL, "data-%X_%d", usage, k) : + kasprintf(GFP_KERNEL, "data-%X", usage); + } + if (!data_field.name) + return -ENOMEM; + + data_field.usage_id = usage; + data_field.is_numeric = (field->flags & HID_MAIN_ITEM_VARIABLE); + if (data_field.is_numeric) { + if (field->unit_exponent > 7 || + field->unit_exponent < -8) + data_field.exp = 0xFF; + else if (field->unit_exponent >= 0) + data_field.exp = field->unit_exponent; + else + data_field.exp = 0x10 - field->unit_exponent; + data_field.unit = field->unit; + } + + data_field.len = (field->report_size >> 3) * field->report_count; + rv = add_data_field(senscol_sensor, &data_field); + senscol_sensor->sample_size += (field->report_size >> 3) * + field->report_count; + + return rv; +} + + static int sensor_hub_probe(struct hid_device *hdev, const struct hid_device_id *id) { @@ -767,6 +1197,12 @@ unsigned long flags; int i; + for (i = 0; i < sensor_hub_count; ++i) + if (hid_sensor_hubs[i] == hdev) { + hid_sensor_hubs[i] = NULL; + break; + } + hid_dbg(hdev, " hardware removed\n"); hid_hw_close(hdev); hid_hw_stop(hdev);