iio: hid-sensor-magn-3d: Scan for usage attributes before setting up iio channels

Scan for and count the HID usage attributes supported by the driver. This allows for the driver to only setup the IIO channels for the sensor usages present in the HID USB reports. Changes from v5 -Fixed kernel panic from invalid pointer dereference -Fixed variable assignment style Signed-off-by: Reyad Attiyat <reyad.attiyat@gmail.com> Acked-by: Srinivas Pandruvada <srinivas.pandruvada@linux.intel.com> Signed-off-by: Jonathan Cameron <jic23@kernel.org>

iio: hid-sensor-magn-3d: Scan for usage attributes before setting up iio channels
Scan for and count the HID usage attributes supported by the driver. This allows for the driver to only setup the IIO channels for the sensor usages present in the HID USB reports. Changes from v5 -Fixed kernel panic from invalid pointer dereference -Fixed variable assignment style Signed-off-by: Reyad Attiyat <reyad.attiyat@gmail.com> Acked-by: Srinivas Pandruvada <srinivas.pandruvada@linux.intel.com> Signed-off-by: Jonathan Cameron <jic23@kernel.org>
Reyad Attiyat · Jonathan Cameron
1 parent 11b8ddab81
Showing 1 changed file with 105 additions and 44 deletions Side-by-side Diff
drivers/iio/magnetometer/hid-sensor-magn-3d.c
@@ -42,7 +42,12 @@
 	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];
+
+	/* dynamically sized array to hold sensor values */
+	u32 *iio_vals;
+	/* array of pointers to sensor value */
+	u32 *magn_val_addr[MAGN_3D_CHANNEL_MAX];
+
 	int scale_pre_decml;
 	int scale_post_decml;
 	int scale_precision;
@@ -66,7 +71,6 @@
 		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,
@@ -76,7 +80,6 @@
 		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,
@@ -86,7 +89,6 @@
 		BIT(IIO_CHAN_INFO_SCALE) |
 		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
 		BIT(IIO_CHAN_INFO_HYSTERESIS),
-		.scan_index = CHANNEL_SCAN_INDEX_Z,
 	}
 };
  
@@ -127,8 +129,8 @@
 		msleep_interruptible(poll_value * 2);
  
 		report_id =
-			magn_state->magn[chan->scan_index].report_id;
-		address = magn_3d_addresses[chan->scan_index];
+			magn_state->magn[chan->address].report_id;
+		address = magn_3d_addresses[chan->address];
 		if (report_id >= 0)
 			*val = sensor_hub_input_attr_get_raw_value(
 				magn_state->common_attributes.hsdev,
@@ -221,8 +223,8 @@
 	dev_dbg(&indio_dev->dev, "magn_3d_proc_event\n");
 	if (atomic_read(&magn_state->common_attributes.data_ready))
 		hid_sensor_push_data(indio_dev,
-				magn_state->magn_val,
-				sizeof(magn_state->magn_val));
+				magn_state->iio_vals,
+				sizeof(magn_state->iio_vals));
  
 	return 0;
 }
  
  
  
  
  
  
  
  
  
@@ -236,52 +238,119 @@
 	struct iio_dev *indio_dev = platform_get_drvdata(priv);
 	struct magn_3d_state *magn_state = iio_priv(indio_dev);
 	int offset;
-	int ret = -EINVAL;
+	int ret = 0;
+	u32 *iio_val = NULL;
  
 	switch (usage_id) {
 	case HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_X_AXIS:
 	case HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_Y_AXIS:
 	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;
+		offset = (usage_id - HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_X_AXIS)
+				+ CHANNEL_SCAN_INDEX_X;
 	break;
 	default:
-		break;
+		return -EINVAL;
 	}
  
+	iio_val = magn_state->magn_val_addr[offset];
+
+	if (iio_val != NULL)
+		*iio_val = *((u32 *)raw_data);
+	else
+		ret = -EINVAL;
+
 	return ret;
 }
  
 /* Parse report which is specific to an usage id*/
 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 **channels,
+				int *chan_count,
 				unsigned usage_id,
 				struct magn_3d_state *st)
 {
-	int ret;
 	int i;
+	int attr_count = 0;
+	struct iio_chan_spec *_channels;
  
-	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);
+	/* Scan for each usage attribute supported */
+	for (i = 0; i < MAGN_3D_CHANNEL_MAX; i++) {
+		int status;
+		u32 address = magn_3d_addresses[i];
+
+		/* Check if usage attribute exists in the sensor hub device */
+		status = sensor_hub_input_get_attribute_info(hsdev,
+			HID_INPUT_REPORT,
+			usage_id,
+			address,
+			&(st->magn[i]));
+		if (!status)
+			attr_count++;
 	}
-	dev_dbg(&pdev->dev, "magn_3d %x:%x, %x:%x, %x:%x\n",
+
+	if (attr_count <= 0) {
+		dev_err(&pdev->dev,
+			"failed to find any supported usage attributes in report\n");
+		return  -EINVAL;
+	}
+
+	dev_dbg(&pdev->dev, "magn_3d Found %d usage attributes\n",
+			attr_count);
+	dev_dbg(&pdev->dev, "magn_3d X: %x:%x Y: %x:%x Z: %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);
  
+	/* Setup IIO channel array */
+	_channels = devm_kcalloc(&pdev->dev, attr_count,
+				sizeof(struct iio_chan_spec),
+				GFP_KERNEL);
+	if (!_channels) {
+		dev_err(&pdev->dev,
+			"failed to allocate space for iio channels\n");
+		return -ENOMEM;
+	}
+
+	st->iio_vals = devm_kcalloc(&pdev->dev, attr_count,
+				sizeof(u32),
+				GFP_KERNEL);
+	if (!st->iio_vals) {
+		dev_err(&pdev->dev,
+			"failed to allocate space for iio values array\n");
+		return -ENOMEM;
+	}
+
+	for (i = 0, *chan_count = 0;
+	i < MAGN_3D_CHANNEL_MAX && *chan_count < attr_count;
+	i++){
+		if (st->magn[i].index >= 0) {
+			/* Setup IIO channel struct */
+			(_channels[*chan_count]) = magn_3d_channels[i];
+			(_channels[*chan_count]).scan_index = *chan_count;
+			(_channels[*chan_count]).address = i;
+
+			/* Set magn_val_addr to iio value address */
+			st->magn_val_addr[i] = &(st->iio_vals[*chan_count]);
+			magn_3d_adjust_channel_bit_mask(_channels,
+							*chan_count,
+							st->magn[i].size);
+			(*chan_count)++;
+		}
+	}
+
+	if (*chan_count <= 0) {
+		dev_err(&pdev->dev,
+			"failed to find any magnetic channels setup\n");
+		return -EINVAL;
+	}
+
+	*channels = _channels;
+
+	dev_dbg(&pdev->dev, "magn_3d Setup %d IIO channels\n",
+			*chan_count);
+
 	st->scale_precision = hid_sensor_format_scale(
 				HID_USAGE_SENSOR_COMPASS_3D,
 				&st->magn[CHANNEL_SCAN_INDEX_X],
@@ -299,7 +368,7 @@
 			st->common_attributes.sensitivity.report_id);
 	}
  
-	return ret;
+	return 0;
 }
  
 /* Function to initialize the processing for usage id */
@@ -311,6 +380,7 @@
 	struct magn_3d_state *magn_state;
 	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
 	struct iio_chan_spec *channels;
+	int chan_count = 0;
  
 	indio_dev = devm_iio_device_alloc(&pdev->dev,
 					  sizeof(struct magn_3d_state));
  
  
@@ -331,22 +401,16 @@
 		return ret;
 	}
  
-	channels = kmemdup(magn_3d_channels, sizeof(magn_3d_channels),
-			   GFP_KERNEL);
-	if (!channels) {
-		dev_err(&pdev->dev, "failed to duplicate channels\n");
-		return -ENOMEM;
-	}
-
-	ret = magn_3d_parse_report(pdev, hsdev, channels,
+	ret = magn_3d_parse_report(pdev, hsdev,
+				&channels, &chan_count,
 				HID_USAGE_SENSOR_COMPASS_3D, magn_state);
 	if (ret) {
-		dev_err(&pdev->dev, "failed to setup attributes\n");
-		goto error_free_dev_mem;
+		dev_err(&pdev->dev, "failed to parse report\n");
+		return ret;
 	}
  
 	indio_dev->channels = channels;
-	indio_dev->num_channels = ARRAY_SIZE(magn_3d_channels);
+	indio_dev->num_channels = chan_count;
 	indio_dev->dev.parent = &pdev->dev;
 	indio_dev->info = &magn_3d_info;
 	indio_dev->name = name;
@@ -356,7 +420,7 @@
 		NULL, NULL);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
-		goto error_free_dev_mem;
+		return ret;
 	}
 	atomic_set(&magn_state->common_attributes.data_ready, 0);
 	ret = hid_sensor_setup_trigger(indio_dev, name,
@@ -390,8 +454,6 @@
 	hid_sensor_remove_trigger(&magn_state->common_attributes);
 error_unreg_buffer_funcs:
 	iio_triggered_buffer_cleanup(indio_dev);
-error_free_dev_mem:
-	kfree(indio_dev->channels);
 	return ret;
 }
  
@@ -406,7 +468,6 @@
 	iio_device_unregister(indio_dev);
 	hid_sensor_remove_trigger(&magn_state->common_attributes);
 	iio_triggered_buffer_cleanup(indio_dev);
-	kfree(indio_dev->channels);
  
 	return 0;
 }
...	...	@@ -42,7 +42,12 @@
42	42	struct hid_sensor_hub_callbacks callbacks;
43	43	struct hid_sensor_common common_attributes;
44	44	struct hid_sensor_hub_attribute_info magn[MAGN_3D_CHANNEL_MAX];
45		- u32 magn_val[MAGN_3D_CHANNEL_MAX];
	45	+
	46	+ /* dynamically sized array to hold sensor values */
	47	+ u32 *iio_vals;
	48	+ /* array of pointers to sensor value */
	49	+ u32 *magn_val_addr[MAGN_3D_CHANNEL_MAX];
	50	+
46	51	int scale_pre_decml;
47	52	int scale_post_decml;
48	53	int scale_precision;
...	...	@@ -66,7 +71,6 @@
66	71	BIT(IIO_CHAN_INFO_SCALE) \|
67	72	BIT(IIO_CHAN_INFO_SAMP_FREQ) \|
68	73	BIT(IIO_CHAN_INFO_HYSTERESIS),
69		- .scan_index = CHANNEL_SCAN_INDEX_X,
70	74	}, {
71	75	.type = IIO_MAGN,
72	76	.modified = 1,
...	...	@@ -76,7 +80,6 @@
76	80	BIT(IIO_CHAN_INFO_SCALE) \|
77	81	BIT(IIO_CHAN_INFO_SAMP_FREQ) \|
78	82	BIT(IIO_CHAN_INFO_HYSTERESIS),
79		- .scan_index = CHANNEL_SCAN_INDEX_Y,
80	83	}, {
81	84	.type = IIO_MAGN,
82	85	.modified = 1,
...	...	@@ -86,7 +89,6 @@
86	89	BIT(IIO_CHAN_INFO_SCALE) \|
87	90	BIT(IIO_CHAN_INFO_SAMP_FREQ) \|
88	91	BIT(IIO_CHAN_INFO_HYSTERESIS),
89		- .scan_index = CHANNEL_SCAN_INDEX_Z,
90	92	}
91	93	};
92	94
...	...	@@ -127,8 +129,8 @@
127	129	msleep_interruptible(poll_value * 2);
128	130
129	131	report_id =
130		- magn_state->magn[chan->scan_index].report_id;
131		- address = magn_3d_addresses[chan->scan_index];
	132	+ magn_state->magn[chan->address].report_id;
	133	+ address = magn_3d_addresses[chan->address];
132	134	if (report_id >= 0)
133	135	*val = sensor_hub_input_attr_get_raw_value(
134	136	magn_state->common_attributes.hsdev,
...	...	@@ -221,8 +223,8 @@
221	223	dev_dbg(&indio_dev->dev, "magn_3d_proc_event\n");
222	224	if (atomic_read(&magn_state->common_attributes.data_ready))
223	225	hid_sensor_push_data(indio_dev,
224		- magn_state->magn_val,
225		- sizeof(magn_state->magn_val));
	226	+ magn_state->iio_vals,
	227	+ sizeof(magn_state->iio_vals));
226	228
227	229	return 0;
228	230	}
229	231
230	232
231	233
232	234
233	235
234	236
235	237
236	238
237	239
...	...	@@ -236,52 +238,119 @@
236	238	struct iio_dev *indio_dev = platform_get_drvdata(priv);
237	239	struct magn_3d_state *magn_state = iio_priv(indio_dev);
238	240	int offset;
239		- int ret = -EINVAL;
	241	+ int ret = 0;
	242	+ u32 *iio_val = NULL;
240	243
241	244	switch (usage_id) {
242	245	case HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_X_AXIS:
243	246	case HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_Y_AXIS:
244	247	case HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_Z_AXIS:
245		- offset = usage_id - HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_X_AXIS;
246		- magn_state->magn_val[CHANNEL_SCAN_INDEX_X + offset] =
247		- (u32 )raw_data;
248		- ret = 0;
	248	+ offset = (usage_id - HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_X_AXIS)
	249	+ + CHANNEL_SCAN_INDEX_X;
249	250	break;
250	251	default:
251		- break;
	252	+ return -EINVAL;
252	253	}
253	254
	255	+ iio_val = magn_state->magn_val_addr[offset];
	256	+
	257	+ if (iio_val != NULL)
	258	+ iio_val = ((u32 *)raw_data);
	259	+ else
	260	+ ret = -EINVAL;
	261	+
254	262	return ret;
255	263	}
256	264
257	265	/* Parse report which is specific to an usage id*/
258	266	static int magn_3d_parse_report(struct platform_device *pdev,
259	267	struct hid_sensor_hub_device *hsdev,
260		- struct iio_chan_spec *channels,
	268	+ struct iio_chan_spec **channels,
	269	+ int *chan_count,
261	270	unsigned usage_id,
262	271	struct magn_3d_state *st)
263	272	{
264		- int ret;
265	273	int i;
	274	+ int attr_count = 0;
	275	+ struct iio_chan_spec *_channels;
266	276
267		- for (i = 0; i <= CHANNEL_SCAN_INDEX_Z; ++i) {
268		- ret = sensor_hub_input_get_attribute_info(hsdev,
269		- HID_INPUT_REPORT,
270		- usage_id,
271		- HID_USAGE_SENSOR_ORIENT_MAGN_FLUX_X_AXIS + i,
272		- &st->magn[CHANNEL_SCAN_INDEX_X + i]);
273		- if (ret < 0)
274		- break;
275		- magn_3d_adjust_channel_bit_mask(channels,
276		- CHANNEL_SCAN_INDEX_X + i,
277		- st->magn[CHANNEL_SCAN_INDEX_X + i].size);
	277	+ /* Scan for each usage attribute supported */
	278	+ for (i = 0; i < MAGN_3D_CHANNEL_MAX; i++) {
	279	+ int status;
	280	+ u32 address = magn_3d_addresses[i];
	281	+
	282	+ /* Check if usage attribute exists in the sensor hub device */
	283	+ status = sensor_hub_input_get_attribute_info(hsdev,
	284	+ HID_INPUT_REPORT,
	285	+ usage_id,
	286	+ address,
	287	+ &(st->magn[i]));
	288	+ if (!status)
	289	+ attr_count++;
278	290	}
279		- dev_dbg(&pdev->dev, "magn_3d %x:%x, %x:%x, %x:%x\n",
	291	+
	292	+ if (attr_count <= 0) {
	293	+ dev_err(&pdev->dev,
	294	+ "failed to find any supported usage attributes in report\n");
	295	+ return -EINVAL;
	296	+ }
	297	+
	298	+ dev_dbg(&pdev->dev, "magn_3d Found %d usage attributes\n",
	299	+ attr_count);
	300	+ dev_dbg(&pdev->dev, "magn_3d X: %x:%x Y: %x:%x Z: %x:%x\n",
280	301	st->magn[0].index,
281	302	st->magn[0].report_id,
282	303	st->magn[1].index, st->magn[1].report_id,
283	304	st->magn[2].index, st->magn[2].report_id);
284	305
	306	+ /* Setup IIO channel array */
	307	+ _channels = devm_kcalloc(&pdev->dev, attr_count,
	308	+ sizeof(struct iio_chan_spec),
	309	+ GFP_KERNEL);
	310	+ if (!_channels) {
	311	+ dev_err(&pdev->dev,
	312	+ "failed to allocate space for iio channels\n");
	313	+ return -ENOMEM;
	314	+ }
	315	+
	316	+ st->iio_vals = devm_kcalloc(&pdev->dev, attr_count,
	317	+ sizeof(u32),
	318	+ GFP_KERNEL);
	319	+ if (!st->iio_vals) {
	320	+ dev_err(&pdev->dev,
	321	+ "failed to allocate space for iio values array\n");
	322	+ return -ENOMEM;
	323	+ }
	324	+
	325	+ for (i = 0, *chan_count = 0;
	326	+ i < MAGN_3D_CHANNEL_MAX && *chan_count < attr_count;
	327	+ i++){
	328	+ if (st->magn[i].index >= 0) {
	329	+ /* Setup IIO channel struct */
	330	+ (_channels[*chan_count]) = magn_3d_channels[i];
	331	+ (_channels[chan_count]).scan_index = chan_count;
	332	+ (_channels[*chan_count]).address = i;
	333	+
	334	+ /* Set magn_val_addr to iio value address */
	335	+ st->magn_val_addr[i] = &(st->iio_vals[*chan_count]);
	336	+ magn_3d_adjust_channel_bit_mask(_channels,
	337	+ *chan_count,
	338	+ st->magn[i].size);
	339	+ (*chan_count)++;
	340	+ }
	341	+ }
	342	+
	343	+ if (*chan_count <= 0) {
	344	+ dev_err(&pdev->dev,
	345	+ "failed to find any magnetic channels setup\n");
	346	+ return -EINVAL;
	347	+ }
	348	+
	349	+ *channels = _channels;
	350	+
	351	+ dev_dbg(&pdev->dev, "magn_3d Setup %d IIO channels\n",
	352	+ *chan_count);
	353	+
285	354	st->scale_precision = hid_sensor_format_scale(
286	355	HID_USAGE_SENSOR_COMPASS_3D,
287	356	&st->magn[CHANNEL_SCAN_INDEX_X],
...	...	@@ -299,7 +368,7 @@
299	368	st->common_attributes.sensitivity.report_id);
300	369	}
301	370
302		- return ret;
	371	+ return 0;
303	372	}
304	373
305	374	/* Function to initialize the processing for usage id */
...	...	@@ -311,6 +380,7 @@
311	380	struct magn_3d_state *magn_state;
312	381	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
313	382	struct iio_chan_spec *channels;
	383	+ int chan_count = 0;
314	384
315	385	indio_dev = devm_iio_device_alloc(&pdev->dev,
316	386	sizeof(struct magn_3d_state));
317	387
318	388
...	...	@@ -331,22 +401,16 @@
331	401	return ret;
332	402	}
333	403
334		- channels = kmemdup(magn_3d_channels, sizeof(magn_3d_channels),
335		- GFP_KERNEL);
336		- if (!channels) {
337		- dev_err(&pdev->dev, "failed to duplicate channels\n");
338		- return -ENOMEM;
339		- }
340		-
341		- ret = magn_3d_parse_report(pdev, hsdev, channels,
	404	+ ret = magn_3d_parse_report(pdev, hsdev,
	405	+ &channels, &chan_count,
342	406	HID_USAGE_SENSOR_COMPASS_3D, magn_state);
343	407	if (ret) {
344		- dev_err(&pdev->dev, "failed to setup attributes\n");
345		- goto error_free_dev_mem;
	408	+ dev_err(&pdev->dev, "failed to parse report\n");
	409	+ return ret;
346	410	}
347	411
348	412	indio_dev->channels = channels;
349		- indio_dev->num_channels = ARRAY_SIZE(magn_3d_channels);
	413	+ indio_dev->num_channels = chan_count;
350	414	indio_dev->dev.parent = &pdev->dev;
351	415	indio_dev->info = &magn_3d_info;
352	416	indio_dev->name = name;
...	...	@@ -356,7 +420,7 @@
356	420	NULL, NULL);
357	421	if (ret) {
358	422	dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
359		- goto error_free_dev_mem;
	423	+ return ret;
360	424	}
361	425	atomic_set(&magn_state->common_attributes.data_ready, 0);
362	426	ret = hid_sensor_setup_trigger(indio_dev, name,
...	...	@@ -390,8 +454,6 @@
390	454	hid_sensor_remove_trigger(&magn_state->common_attributes);
391	455	error_unreg_buffer_funcs:
392	456	iio_triggered_buffer_cleanup(indio_dev);
393		-error_free_dev_mem:
394		- kfree(indio_dev->channels);
395	457	return ret;
396	458	}
397	459
...	...	@@ -406,7 +468,6 @@
406	468	iio_device_unregister(indio_dev);
407	469	hid_sensor_remove_trigger(&magn_state->common_attributes);
408	470	iio_triggered_buffer_cleanup(indio_dev);
409		- kfree(indio_dev->channels);
410	471
411	472	return 0;
412	473	}