Commit 5058355c authored by Zhang Lixu's avatar Zhang Lixu Committed by Carles Cufi
Browse files

sensing: add rtio implement for sensing subsys 

1. add function sensing_iodev_submit and defined struct
   sensing_submit_config
2. fix the issue of phy_3d_sensor

Signed-off-by: default avatarZhang Lixu <lixu.zhang@intel.com>
parent da511800

include/zephyr/sensing/sensing_sensor.h

+18 −3
Changes for include/zephyr/sensing/sensing_sensor.h: 18 added lines, 3 removed lines.
Original line number Diff line number Diff line
@@ -153,13 +153,28 @@ extern struct sensing_sensor SENSING_SENSOR_SOURCE_NAME(idx, node); \ 
				(,), node, cb_list_ptr)			\

	};



struct sensing_submit_config {

	enum sensor_channel chan;

	const int info_index;

	const bool is_streaming;

};



extern const struct rtio_iodev_api __sensing_iodev_api;



#define SENSING_SUBMIT_CFG_NAME(node, idx)						\

	_CONCAT(_CONCAT(__sensing_submit_cfg_, idx), DEVICE_DT_NAME_GET(node))



#define SENSING_SENSOR_IODEV_NAME(node, idx)						\

	_CONCAT(_CONCAT(__sensing_iodev_, idx), DEVICE_DT_NAME_GET(node))



#define SENSING_SENSOR_IODEV_DEFINE(node, idx)						\

	COND_CODE_1(DT_PROP(node, stream_mode),						\

		(SENSOR_DT_STREAM_IODEV(SENSING_SENSOR_IODEV_NAME(node, idx), node)),	\

		(SENSOR_DT_READ_IODEV(SENSING_SENSOR_IODEV_NAME(node, idx), node)));

	static struct sensing_submit_config SENSING_SUBMIT_CFG_NAME(node, idx) = {	\

		.is_streaming = DT_PROP(node, stream_mode),				\

		.info_index = idx,							\

	};										\

	RTIO_IODEV_DEFINE(SENSING_SENSOR_IODEV_NAME(node, idx),				\

			  &__sensing_iodev_api,						\

			  &SENSING_SUBMIT_CFG_NAME(node, idx));



#define SENSING_SENSOR_NAME(node, idx)					\

	_CONCAT(_CONCAT(__sensing_sensor_, idx), DEVICE_DT_NAME_GET(node))

subsys/sensing/sensing_sensor.c

+17 −0
Changes for subsys/sensing/sensing_sensor.c: 17 added lines, 0 removed lines.
Original line number Diff line number Diff line
@@ -13,6 +13,23 @@ 


LOG_MODULE_DECLARE(sensing, CONFIG_SENSING_LOG_LEVEL);



static void sensing_iodev_submit(struct rtio_iodev_sqe *iodev_sqe)

{

	struct sensing_sensor *sensor = (struct sensing_sensor *)iodev_sqe->sqe.userdata;

	const struct device *dev = sensor->dev;

	const struct sensor_driver_api *api = dev->api;



	if (api->submit != NULL) {

		api->submit(dev, iodev_sqe);

	} else {

		rtio_iodev_sqe_err(iodev_sqe, -ENOTSUP);

	}

}



const struct rtio_iodev_api __sensing_iodev_api = {

	.submit = sensing_iodev_submit,

};



int sensing_sensor_get_reporters(const struct device *dev, int type,

		sensing_sensor_handle_t *reporter_handles,

		int max_handles)

subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.c

+28 −43
Changes for subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.c: 28 added lines, 43 removed lines.
Original line number Diff line number Diff line
@@ -102,19 +102,22 @@ static int phy_3d_sensor_init(const struct device *dev) 
{

	const struct phy_3d_sensor_config *cfg = dev->config;

	struct phy_3d_sensor_data *data = dev->data;

	int i;



	switch (cfg->sensor_type) {

	for (i = 0; i < cfg->sensor_num; i++) {

		switch (cfg->sensor_types[i]) {

		case SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D:

		data->custom = &custom_accel;

			data->customs[i] = &custom_accel;

		break;

		case SENSING_SENSOR_TYPE_MOTION_GYROMETER_3D:

		data->custom = &custom_gyro;

			data->customs[i] = &custom_gyro;

		break;

		default:

			LOG_ERR("phy_3d_sensor doesn't support sensor type %d",

				cfg->sensor_type);

			cfg->sensor_types[i]);

			return -ENOTSUP;

		}

	}



	LOG_INF("%s: Underlying device: %s", dev->name, cfg->hw_dev->name);
@@ -126,38 +129,12 @@ static int phy_3d_sensor_attr_set_hyst(const struct device *dev, 
		const struct sensor_value *val)

{

	const struct phy_3d_sensor_config *cfg = dev->config;

	struct phy_3d_sensor_data *data = dev->data;

	int index = chan - SENSOR_CHAN_PRIV_START;

	int i, min = 0;



	if (index >= 0 && index < ARRAY_SIZE(data->sensitivities)) {

		data->sensitivities[index] = *val;

	} else if (index == SENSING_SENSITIVITY_INDEX_ALL) {

		for (i = 0; i < ARRAY_SIZE(data->sensitivities); ++i) {

			data->sensitivities[i] = *val;

		}

		min = ARRAY_SIZE(data->sensitivities) - 1;

	} else {

		LOG_ERR("%s: set sensitivity: invalid index: %d",

				dev->name, index);

		return -EINVAL;

	}



	for (i = min + 1; i < ARRAY_SIZE(data->sensitivities); ++i) {

		if ((data->sensitivities[i].val1 <

					data->sensitivities[min].val1) ||

				(data->sensitivities[i].val1 ==

				 data->sensitivities[min].val1 &&

				 data->sensitivities[i].val2 <

				 data->sensitivities[min].val2)) {

			min = i;

		}

	if (chan == SENSOR_CHAN_ACCEL_XYZ || chan == SENSOR_CHAN_GYRO_XYZ) {

		return sensor_attr_set(cfg->hw_dev, chan, SENSOR_ATTR_SLOPE_TH, val);

	} else {

		return sensor_attr_set(cfg->hw_dev, chan, SENSOR_ATTR_HYSTERESIS, val);

	}



	chan = data->custom->chan_all;



	return sensor_attr_set(cfg->hw_dev, chan, SENSOR_ATTR_SLOPE_TH,

			&data->sensitivities[min]);

}



static int phy_3d_sensor_attr_set(const struct device *dev,
@@ -166,7 +143,6 @@ static int phy_3d_sensor_attr_set(const struct device *dev, 
		const struct sensor_value *val)

{

	const struct phy_3d_sensor_config *cfg = dev->config;

	struct phy_3d_sensor_data *data = dev->data;

	int ret = 0;



	switch (attr) {
@@ -175,7 +151,6 @@ static int phy_3d_sensor_attr_set(const struct device *dev, 
		break;



	default:

		chan = data->custom->chan_all;

		ret = sensor_attr_set(cfg->hw_dev, chan, attr, val);

		break;

	}
@@ -187,8 +162,10 @@ static int phy_3d_sensor_attr_set(const struct device *dev, 
static int phy_3d_sensor_submit(const struct device *dev,

		struct rtio_iodev_sqe *sqe)

{

	struct sensing_submit_config *config = (struct sensing_submit_config *)sqe->sqe.iodev->data;

	const struct phy_3d_sensor_config *cfg = dev->config;

	struct phy_3d_sensor_data *data = dev->data;

	const struct phy_3d_sensor_custom *custom = data->customs[config->info_index];

	struct sensing_sensor_value_3d_q31 *sample;

	struct sensor_value value[PHY_3D_SENSOR_CHANNEL_NUM];

	uint32_t buffer_len;
@@ -201,14 +178,14 @@ static int phy_3d_sensor_submit(const struct device *dev, 
		return ret;

	}



	ret = sensor_sample_fetch_chan(cfg->hw_dev, data->custom->chan_all);

	ret = sensor_sample_fetch_chan(cfg->hw_dev, custom->chan_all);

	if (ret) {

		LOG_ERR("%s: sample fetch failed: %d", dev->name, ret);

		rtio_iodev_sqe_err(sqe, ret);

		return ret;

	}



	ret = sensor_channel_get(cfg->hw_dev, data->custom->chan_all, value);

	ret = sensor_channel_get(cfg->hw_dev, custom->chan_all, value);

	if (ret) {

		LOG_ERR("%s: channel get failed: %d", dev->name, ret);

		rtio_iodev_sqe_err(sqe, ret);
@@ -216,12 +193,11 @@ static int phy_3d_sensor_submit(const struct device *dev, 
	}



	for (i = 0; i < ARRAY_SIZE(value); ++i) {

		sample->readings[0].v[i] =

			data->custom->sensor_value_to_q31(&value[i]);

		sample->readings[0].v[i] = custom->sensor_value_to_q31(&value[i]);

	}



	sample->header.reading_count = 1;

	sample->shift = data->custom->shift;

	sample->shift = custom->shift;



	LOG_DBG("%s: Sample data:\t x: %d, y: %d, z: %d",

			dev->name,
@@ -238,6 +214,7 @@ static const struct sensor_driver_api phy_3d_sensor_api = { 
	.submit = phy_3d_sensor_submit,

};



/* To be removed */

static const struct sensing_sensor_register_info phy_3d_sensor_reg = {

	.flags = SENSING_SENSOR_FLAG_REPORT_ON_CHANGE,

	.sample_size = sizeof(struct sensing_sensor_value_3d_q31),
@@ -246,12 +223,20 @@ static const struct sensing_sensor_register_info phy_3d_sensor_reg = { 
};



#define SENSING_PHY_3D_SENSOR_DT_DEFINE(_inst)						\

	static struct phy_3d_sensor_data _CONCAT(data, _inst);		\

	static struct rtio_iodev_sqe _CONCAT(sqes, _inst)[DT_INST_PROP_LEN(_inst,	\

			sensor_types)];							\

	static const struct phy_3d_sensor_custom *_CONCAT(customs, _inst)		\

			[DT_INST_PROP_LEN(_inst, sensor_types)];			\

	static struct phy_3d_sensor_data _CONCAT(data, _inst) = {			\

		.sqes = _CONCAT(sqes, _inst),						\

		.customs = _CONCAT(customs, _inst),					\

	};										\

	static const struct phy_3d_sensor_config _CONCAT(cfg, _inst) = {		\

		.hw_dev = DEVICE_DT_GET(						\

				DT_PHANDLE(DT_DRV_INST(_inst),				\

				underlying_device)),					\

		.sensor_type = DT_PROP_BY_IDX(DT_DRV_INST(_inst), sensor_types, 0),\

		.sensor_num = DT_INST_PROP_LEN(_inst, sensor_types),			\

		.sensor_types = DT_PROP(DT_DRV_INST(_inst), sensor_types),		\

	};										\

	SENSING_SENSORS_DT_INST_DEFINE(_inst, &phy_3d_sensor_reg, NULL,			\

		&phy_3d_sensor_init, NULL,						\

subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.h

+4 −2
Changes for subsys/sensing/sensor/phy_3d_sensor/phy_3d_sensor.h: 4 added lines, 2 removed lines.
Original line number Diff line number Diff line
@@ -20,13 +20,15 @@ struct phy_3d_sensor_custom { 
};



struct phy_3d_sensor_data {

	const struct phy_3d_sensor_custom *custom;

	struct sensor_value sensitivities[PHY_3D_SENSOR_CHANNEL_NUM];

	const struct phy_3d_sensor_custom **customs;

	struct rtio_iodev_sqe *sqes;

};



struct phy_3d_sensor_config {

	const struct device *hw_dev;

	const int32_t sensor_type;

	const int sensor_num;

	const int32_t sensor_types[];

};



#endif

subsys/sensing/sensor_mgmt.c

+27 −8
Changes for subsys/sensing/sensor_mgmt.c: 27 added lines, 8 removed lines.
Original line number Diff line number Diff line
@@ -29,6 +29,20 @@ RTIO_DEFINE_WITH_MEMPOOL(sensing_rtio_ctx, CONFIG_SENSING_RTIO_SQE_NUM, 
		CONFIG_SENSING_RTIO_BLOCK_COUNT,

		CONFIG_SENSING_RTIO_BLOCK_SIZE, 4);



static enum sensor_channel sensing_sensor_type_to_chan(const int32_t type)

{

	switch (type) {

	case SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D:

		return SENSOR_CHAN_ACCEL_XYZ;

	case SENSING_SENSOR_TYPE_MOTION_GYROMETER_3D:

		return SENSOR_CHAN_GYRO_XYZ;

	default:

		break;

	}



	return SENSOR_CHAN_PRIV_START;

}



/* sensor_later_config including arbitrate/set interval/sensitivity

 */

static uint32_t arbitrate_interval(struct sensing_sensor *sensor)
@@ -61,7 +75,7 @@ static uint32_t arbitrate_interval(struct sensing_sensor *sensor) 


static int set_arbitrate_interval(struct sensing_sensor *sensor, uint32_t interval)

{

	struct sensor_read_config *config = sensor->iodev->data;

	struct sensing_submit_config *config = sensor->iodev->data;

	struct sensor_value odr = {0};

	int ret;
@@ -75,8 +89,7 @@ static int set_arbitrate_interval(struct sensing_sensor *sensor, uint32_t interv 
		odr.val2 = (uint64_t)USEC_PER_SEC * 1000000 / interval % 1000000;

	}



	/* The SENSOR_CHAN_MAX should be overridden by sensing sensor driver */

	ret = sensor_attr_set(sensor->dev, SENSOR_CHAN_MAX,

	ret = sensor_attr_set(sensor->dev, config->chan,

			SENSOR_ATTR_SAMPLING_FREQUENCY, &odr);

	if (ret) {

		LOG_ERR("%s set attr freq failed:%d", sensor->dev->name, ret);
@@ -143,16 +156,18 @@ static uint32_t arbitrate_sensitivity(struct sensing_sensor *sensor, int index) 


static int set_arbitrate_sensitivity(struct sensing_sensor *sensor, int index, uint32_t sensitivity)

{

	struct sensing_submit_config *config = (struct sensing_submit_config *)sensor->iodev->data;

	struct sensor_value threshold = {.val1 = sensitivity};

	int i;



	/* update sensor sensitivity */

	sensor->sensitivity[index] = sensitivity;



	/* The SENSOR_CHAN_PRIV_START should be overridden by sensing sensor

	 * driver, SENSOR_ATTR_HYSTERESIS can be overridden for different

	 * sensora type.

	 */

	return sensor_attr_set(sensor->dev, SENSOR_CHAN_PRIV_START + index,

	for (i = 0; i < sensor->sensitivity_count; i++) {

		threshold.val1 = MIN(sensor->sensitivity[i], threshold.val1);

	}



	return sensor_attr_set(sensor->dev, config->chan,

			SENSOR_ATTR_HYSTERESIS, &threshold);

}
@@ -275,6 +290,7 @@ static void sensing_sensor_polling_timer(struct k_timer *timer_id) 


static int init_sensor(struct sensing_sensor *sensor)

{

	struct sensing_submit_config *config;

	struct sensing_connection *conn;

	int i;
@@ -293,6 +309,9 @@ static int init_sensor(struct sensing_sensor *sensor) 
			conn->source->dev->name, sensor->dev->name, i, conn);

	}



	config = sensor->iodev->data;

	config->chan = sensing_sensor_type_to_chan(sensor->info->type);



	return 0;

}

CRA Git | Maintained and supported by SUSTech CRA and CCSE