Updated magn_3d_channel enum for all possible north channels
Added functions to setup iio_chan_spec array depending on which hid usage reports are found
Renamed magn_val to iio_val to differentiate the index being used
Updated magn_3d_state struct to hold pointer array (magn_val_addr[]) which points to iio_val[]
Updated magn_3d_parse_report to scan for all compass usages and create iio channels for each
Signed-off-by: Reyad Attiyat <reyad.attiyat@xxxxxxxxx>
---
drivers/iio/magnetometer/hid-sensor-magn-3d.c | 271 +++++++++++++++++---------
1 file changed, 177 insertions(+), 94 deletions(-)
diff --git a/drivers/iio/magnetometer/hid-sensor-magn-3d.c b/drivers/iio/magnetometer/hid-sensor-magn-3d.c
index 6d162b7..32f4d90 100644
--- a/drivers/iio/magnetometer/hid-sensor-magn-3d.c
+++ b/drivers/iio/magnetometer/hid-sensor-magn-3d.c
@@ -34,63 +34,54 @@ enum magn_3d_channel {
CHANNEL_SCAN_INDEX_X,
CHANNEL_SCAN_INDEX_Y,
CHANNEL_SCAN_INDEX_Z,
+ CHANNEL_SCAN_INDEX_NORTH_MAGN,
+ CHANNEL_SCAN_INDEX_NORTH_TRUE,
+ CHANNEL_SCAN_INDEX_NORTH_TILT_COMP,
+ CHANNEL_SCAN_INDEX_NORTH_TRUE_TILT_COMP,
MAGN_3D_CHANNEL_MAX,
};
+#define IIO_CHANNEL_MAX MAGN_3D_CHANNEL_MAX
+
struct magn_3d_state {
struct hid_sensor_hub_callbacks callbacks;
struct hid_sensor_common common_attributes;
struct hid_sensor_hub_attribute_info magn[MAGN_3D_CHANNEL_MAX];
- u32 magn_val[MAGN_3D_CHANNEL_MAX];
-};
+ u32 *magn_val_addr[MAGN_3D_CHANNEL_MAX];
-static const u32 magn_3d_addresses[MAGN_3D_CHANNEL_MAX] = {
- HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_X_AXIS,
- HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_Y_AXIS,
- HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_Z_AXIS
+ u32 iio_val[IIO_CHANNEL_MAX];
+ int num_iio_channels;
};
-/* Channel definitions */
-static const struct iio_chan_spec magn_3d_channels[] = {
- {
- .type = IIO_MAGN,
- .modified = 1,
- .channel2 = IIO_MOD_X,
- .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
- BIT(IIO_CHAN_INFO_SCALE) |
- BIT(IIO_CHAN_INFO_SAMP_FREQ) |
- BIT(IIO_CHAN_INFO_HYSTERESIS),
- .scan_index = CHANNEL_SCAN_INDEX_X,
- }, {
- .type = IIO_MAGN,
- .modified = 1,
- .channel2 = IIO_MOD_Y,
- .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
- BIT(IIO_CHAN_INFO_SCALE) |
- BIT(IIO_CHAN_INFO_SAMP_FREQ) |
- BIT(IIO_CHAN_INFO_HYSTERESIS),
- .scan_index = CHANNEL_SCAN_INDEX_Y,
- }, {
- .type = IIO_MAGN,
- .modified = 1,
- .channel2 = IIO_MOD_Z,
- .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
- BIT(IIO_CHAN_INFO_SCALE) |
- BIT(IIO_CHAN_INFO_SAMP_FREQ) |
- BIT(IIO_CHAN_INFO_HYSTERESIS),
- .scan_index = CHANNEL_SCAN_INDEX_Z,
+/* Find index into magn_3d_state magn[] and magn_val_addr[] from HID Usage */
+static int magn_3d_usage_id_to_chan_index(unsigned usage_id){
+ int offset = -1;
+
+ switch (usage_id) {
+ case HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_X_AXIS:
+ offset = CHANNEL_SCAN_INDEX_X;
+ break;
+ case HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_Y_AXIS:
+ offset = CHANNEL_SCAN_INDEX_Y;
+ break;
+ case HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_Z_AXIS:
+ offset = CHANNEL_SCAN_INDEX_Z;
+ break;
+ case HID_USAGE_SENSOR_ORIENT_COMP_MAGN_NORTH:
+ offset = CHANNEL_SCAN_INDEX_NORTH_TILT_COMP;
+ break;
+ case HID_USAGE_SENSOR_ORIENT_COMP_TRUE_NORTH:
+ offset = CHANNEL_SCAN_INDEX_NORTH_TRUE_TILT_COMP;
+ break;
+ case HID_USAGE_SENSOR_ORIENT_MAGN_NORTH:
+ offset = CHANNEL_SCAN_INDEX_NORTH_MAGN;
+ break;
+ case HID_USAGE_SENSOR_ORIENT_TRUE_NORTH:
+ offset = CHANNEL_SCAN_INDEX_NORTH_TRUE;
+ break;
}
-};
-/* Adjust channel real bits based on report descriptor */
-static void magn_3d_adjust_channel_bit_mask(struct iio_chan_spec *channels,
- int channel, int size)
-{
- channels[channel].scan_type.sign = 's';
- /* Real storage bits will change based on the report desc. */
- channels[channel].scan_type.realbits = size * 8;
- /* Maximum size of a sample to capture is u32 */
- channels[channel].scan_type.storagebits = sizeof(u32) * 8;
+ return offset;
}
/* Channel read_raw handler */
@@ -101,21 +92,31 @@ static int magn_3d_read_raw(struct iio_dev *indio_dev,
{
struct magn_3d_state *magn_state = iio_priv(indio_dev);
int report_id = -1;
- u32 address;
+ unsigned usage_id;
+ int chan_index = -1;
int ret;
int ret_type;
+ dev_dbg(&indio_dev->dev, "magn_3d_read_raw\n");
+
*val = 0;
*val2 = 0;
switch (mask) {
case 0:
+ /* We store the HID usage ID of the iio channel
+ * in its address field
+ */
+ usage_id = chan->address;
+ chan_index = magn_3d_usage_id_to_chan_index(usage_id);
+ if(chan_index < 0)
+ return -EINVAL;
+
report_id =
- magn_state->magn[chan->scan_index].report_id;
- address = magn_3d_addresses[chan->scan_index];
+ magn_state->magn[chan_index].report_id;
if (report_id >= 0)
*val = sensor_hub_input_attr_get_raw_value(
magn_state->common_attributes.hsdev,
- HID_USAGE_SENSOR_COMPASS_3D, address,
+ HID_USAGE_SENSOR_COMPASS_3D, usage_id,
report_id);
else {
*val = 0;
@@ -202,12 +203,13 @@ static int magn_3d_proc_event(struct hid_sensor_hub_device *hsdev,
magn_state->common_attributes.data_ready);
if (magn_state->common_attributes.data_ready)
hid_sensor_push_data(indio_dev,
- magn_state->magn_val,
- sizeof(magn_state->magn_val));
+ &(magn_state->iio_val),
+ sizeof(magn_state->iio_val));
return 0;
}
+
/* Capture samples in local storage */
static int magn_3d_capture_sample(struct hid_sensor_hub_device *hsdev,
unsigned usage_id,
@@ -217,63 +219,143 @@ static int magn_3d_capture_sample(struct hid_sensor_hub_device *hsdev,
struct iio_dev *indio_dev = platform_get_drvdata(priv);
struct magn_3d_state *magn_state = iio_priv(indio_dev);
int offset;
+ u32 *magn_val;
int ret = -EINVAL;
- switch (usage_id) {
+ offset = magn_3d_usage_id_to_chan_index(usage_id);
+ if(offset < 0)
+ return ret;
+
+ magn_val = magn_state->magn_val_addr[offset];
+ if(!magn_val)
+ return ret;
+
+ *(magn_val) = *(u32 *)raw_data;
+
+ return 0;
+}
+
+/* Setup the iio_chan_spec for HID Usage ID */
+static int magn_3d_setup_new_iio_chan(struct hid_sensor_hub_device *hsdev,
+ unsigned usage_id,
+ unsigned attr_usage_id,
+ struct iio_chan_spec *iio_chans,
+ struct magn_3d_state *st)
+{
+ int ret = -1;
+ int iio_index;
+ int magn_index;
+ struct iio_chan_spec *channel;
+
+ /* Setup magn_3d_state for new channel */
+ magn_index = magn_3d_usage_id_to_chan_index(attr_usage_id);
+ if(magn_index < 0 || magn_index >= MAGN_3D_CHANNEL_MAX){
+ return -1;
+ }
+
+ /* Check if usage attribute exists in the sensor hub device */
+ ret = sensor_hub_input_get_attribute_info(hsdev,
+ HID_INPUT_REPORT,
+ usage_id,
+ attr_usage_id,
+ &(st->magn[magn_index]));
+ if(ret != 0){
+ /* Usage not found. Nothing to setup.*/
+ return 0;
+ }
+
+ iio_index = st->num_iio_channels;
+ if(iio_index < 0 || iio_index >= IIO_CHANNEL_MAX){
+ return -2;
+ }
+
+ /* Check if this usage hasn't been setup */
+ if(st->magn_val_addr[magn_index] != NULL){
+ return -3;
+ }
+ st->magn_val_addr[magn_index] = &(st->iio_val[iio_index]);
+
+ /* Setup IIO Channel spec */
+ channel = &(iio_chans[iio_index]);
+
+ channel->type = IIO_MAGN;
+ channel->address = attr_usage_id;
+ channel->modified = 1;
+
+ switch (attr_usage_id){
+ case HID_USAGE_SENSOR_ORIENT_COMP_MAGN_NORTH:
+ channel->channel2 = IIO_MOD_NORTH_MAGN_TILT_COMP;
+ break;
+ case HID_USAGE_SENSOR_ORIENT_COMP_TRUE_NORTH:
+ channel->channel2 = IIO_MOD_NORTH_TRUE_TILT_COMP;
+ break;
+ case HID_USAGE_SENSOR_ORIENT_MAGN_NORTH:
+ channel->channel2 = IIO_MOD_NORTH_MAGN;
+ break;
+ case HID_USAGE_SENSOR_ORIENT_TRUE_NORTH:
+ channel->channel2 = IIO_MOD_NORTH_TRUE;
+ break;
case HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_X_AXIS:
+ channel->channel2 = IIO_MOD_X;
+ break;
case HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_Y_AXIS:
+ channel->channel2 = IIO_MOD_Y;
+ break;
case HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_Z_AXIS:
- offset = usage_id - HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_X_AXIS;
- magn_state->magn_val[CHANNEL_SCAN_INDEX_X + offset] =
- *(u32 *)raw_data;
- ret = 0;
- break;
- default:
+ channel->channel2 = IIO_MOD_Z;
break;
+ default:
+ return -4;
}
+ channel->info_mask_shared_by_type =
+ BIT(IIO_CHAN_INFO_OFFSET) |
+ BIT(IIO_CHAN_INFO_SCALE) |
+ BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+ BIT(IIO_CHAN_INFO_HYSTERESIS);
+
+ channel->scan_type.sign = 's';
+ /* Real storage bits will change based on the report desc. */
+ channel->scan_type.realbits = st->magn[magn_index].size * 8;
+ /* Maximum size of a sample to capture is u32 */
+ channel->scan_type.storagebits = sizeof(u32) * 8;
+
+ (st->num_iio_channels)++;
+ ret = 0;
+
return ret;
}
-/* Parse report which is specific to an usage id*/
+/* Read the HID reports and setup IIO Channels */
static int magn_3d_parse_report(struct platform_device *pdev,
struct hid_sensor_hub_device *hsdev,
- struct iio_chan_spec *channels,
+ struct iio_chan_spec *iio_chans,
unsigned usage_id,
struct magn_3d_state *st)
{
- int ret;
+ int ret = 0;
int i;
- for (i = 0; i <= CHANNEL_SCAN_INDEX_Z; ++i) {
- ret = sensor_hub_input_get_attribute_info(hsdev,
- HID_INPUT_REPORT,
- usage_id,
- HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_X_AXIS + i,
- &st->magn[CHANNEL_SCAN_INDEX_X + i]);
- if (ret < 0)
- break;
- magn_3d_adjust_channel_bit_mask(channels,
- CHANNEL_SCAN_INDEX_X + i,
- st->magn[CHANNEL_SCAN_INDEX_X + i].size);
- }
- dev_dbg(&pdev->dev, "magn_3d %x:%x, %x:%x, %x:%x\n",
- st->magn[0].index,
- st->magn[0].report_id,
- st->magn[1].index, st->magn[1].report_id,
- st->magn[2].index, st->magn[2].report_id);
-
- /* Set Sensitivity field ids, when there is no individual modifier */
- if (st->common_attributes.sensitivity.index < 0) {
- sensor_hub_input_get_attribute_info(hsdev,
- HID_FEATURE_REPORT, usage_id,
- HID_USAGE_SENSOR_DATA_MOD_CHANGE_SENSITIVITY_ABS |
- HID_USAGE_SENSOR_DATA_ORIENTATION,
- &st->common_attributes.sensitivity);
- dev_dbg(&pdev->dev, "Sensitivity index:report %d:%d\n",
- st->common_attributes.sensitivity.index,
- st->common_attributes.sensitivity.report_id);
- }
+ dev_dbg(&pdev->dev, "magn_3d_parse_reports Usage ID: %x\n", usage_id);
+ for(i = HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_X_AXIS;
+ i <= HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_Z_AXIS && ret == 0;
+ i++)
+ ret = magn_3d_setup_new_iio_chan(hsdev,
+ usage_id,
+ i,
+ iio_chans,
+ st);
+
+ dev_dbg(&pdev->dev, "magn_3d_parse_reports Found %d\n magnetic axis. Returned: %d", st->num_iio_channels, ret);
+ for(i = HID_USAGE_SENSOR_ORIENT_COMP_MAGN_NORTH;
+ i <= HID_USAGE_SENSOR_ORIENT_TRUE_NORTH && ret == 0;
+ i++)
+ ret = magn_3d_setup_new_iio_chan(hsdev,
+ usage_id,
+ i,
+ iio_chans,
+ st);
+ dev_dbg(&pdev->dev, "magn_3d_parse_reports Found %d\n magnetic channels. Returned: %d", st->num_iio_channels, ret);
return ret;
}
@@ -307,10 +389,11 @@ static int hid_magn_3d_probe(struct platform_device *pdev)
return ret;
}
- channels = kmemdup(magn_3d_channels, sizeof(magn_3d_channels),
- GFP_KERNEL);
+ channels = kcalloc(MAGN_3D_CHANNEL_MAX,
+ sizeof(struct iio_chan_spec),
+ GFP_KERNEL);
if (!channels) {
- dev_err(&pdev->dev, "failed to duplicate channels\n");
+ dev_err(&pdev->dev, "failed to allocate memory for iio channel\n");
return -ENOMEM;
}
@@ -322,7 +405,7 @@ static int hid_magn_3d_probe(struct platform_device *pdev)
}
indio_dev->channels = channels;
- indio_dev->num_channels = ARRAY_SIZE(magn_3d_channels);
+ indio_dev->num_channels = magn_state->num_iio_channels;
indio_dev->dev.parent = &pdev->dev;
indio_dev->info = &magn_3d_info;
indio_dev->name = name;
--
1.9.3
--
To unsubscribe from this list: send the line "unsubscribe linux-iio" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at http://vger.kernel.org/majordomo-info.html
