sensors: add bin_data_age attribute

JakubVanek · JakubVanek · commit f9c4a1048e51 · 2020-08-09T23:17:38.000+02:00
diff --git a/brickpi/brickpi_i2c_sensor.c b/brickpi/brickpi_i2c_sensor.c
@@ -76,6 +76,7 @@ static int brickpi_i2c_sensor_set_mode(void *context, u8 mode)
 		return err;
 
 	lego_port_set_raw_data_ptr_and_func(port, mode_info->raw_data, size,
+					    &mode_info->last_changed_time,
 					    NULL, NULL);
 
 	return 0;
@@ -205,7 +206,8 @@ static int brickpi_i2c_sensor_remove(struct lego_device *ldev)
 {
 	struct brickpi_i2c_sensor_data *data = dev_get_drvdata(&ldev->dev);
 
-	lego_port_set_raw_data_ptr_and_func(ldev->port, NULL, 0, NULL, NULL);
+	lego_port_set_raw_data_ptr_and_func(ldev->port, NULL, 0, NULL,
+					    NULL, NULL);
 	unregister_lego_sensor(&data->sensor);
 	dev_set_drvdata(&ldev->dev, NULL);
 	kfree(data->sensor.mode_info);
diff --git a/brickpi/brickpi_serdev.c b/brickpi/brickpi_serdev.c
@@ -401,11 +401,25 @@ int brickpi_get_values(struct brickpi_channel_data *ch_data)
 
 		if (port->sensor_type == BRICKPI_SENSOR_TYPE_NXT_I2C
 		   || port->sensor_type == BRICKPI_SENSOR_TYPE_NXT_I2C_9V) {
-			memcpy(raw_data, port->i2c_msg[0].read_data,
-				port->i2c_msg[0].read_size);
+
+			if (port->port.last_changed_time
+			   && memcmp(raw_data, port->i2c_msg[0].read_data,
+				     port->i2c_msg[0].read_size) != 0) {
+				*port->port.last_changed_time = ktime_get();
+				memcpy(raw_data, port->i2c_msg[0].read_data,
+				       port->i2c_msg[0].read_size);
+			}
+
+
 		} else {
-			memcpy(raw_data, sensor_values,
-			       sizeof(s32) * NUM_BRICKPI_SENSOR_VALUES);
+			int bytes = sizeof(s32) * NUM_BRICKPI_SENSOR_VALUES;
+
+			if (port->port.last_changed_time
+			   && memcmp(raw_data, sensor_values, bytes) != 0) {
+				*port->port.last_changed_time = ktime_get();
+				memcpy(raw_data, sensor_values, bytes);
+			}
+
 		}
 		lego_port_call_raw_data_func(&port->port);
 	}
diff --git a/brickpi3/brickpi3_ports_in.c b/brickpi3/brickpi3_ports_in.c
@@ -165,9 +165,20 @@ static void brickpi3_in_port_poll_work(struct work_struct *work)
 	struct brickpi3_in_port *data =
 		container_of(work, struct brickpi3_in_port, poll_work);
 	u8 *raw_data = data->port.raw_data;
+	ktime_t *last_changed_ptr = data->port.last_changed_time;
+	u8 old_data[32];
+	int check_size = 0;
 	u8 msg[16];
 	int ret;
 
+	if (raw_data && last_changed_ptr) {
+		check_size = data->port.raw_data_size <= 32
+				? data->port.raw_data_size : 32;
+	}
+
+	if (check_size)
+		memcpy(old_data, raw_data, check_size);
+
 	switch (data->sensor_type) {
 	case BRICKPI3_SENSOR_TYPE_CUSTOM:
 		ret = brickpi3_read_sensor(data->bp, data->address, data->index,
@@ -277,6 +288,11 @@ static void brickpi3_in_port_poll_work(struct work_struct *work)
 		return;
 	}
 
+	if (check_size) {
+		if (memcmp(old_data, raw_data, check_size) != 0)
+			*last_changed_ptr = ktime_get();
+	}
+
 	if (raw_data) {
 		lego_port_call_raw_data_func(&data->port);
 	}
diff --git a/ev3/ev3_ports_in.c b/ev3/ev3_ports_in.c
@@ -526,16 +526,30 @@ static struct lego_port_nxt_analog_ops ev3_input_port_nxt_analog_ops = {
 
 static void ev3_input_port_nxt_analog_cb(struct ev3_input_port_data *data)
 {
-	if (data->port.raw_data)
-		*(s32 *)data->port.raw_data = data->pin1_mv;
+	s32 new_value = data->pin1_mv;
+	s32 *raw_data = (s32 *)data->port.raw_data;
+
+	if (raw_data) {
+		if (*raw_data != new_value && data->port.last_changed_time)
+			*data->port.last_changed_time = ktime_get();
+
+		*raw_data = new_value;
+	}
 	if (data->port.notify_raw_data_func)
 		data->port.notify_raw_data_func(data->port.notify_raw_data_context);
 }
 
 static void ev3_input_port_ev3_analog_cb(struct ev3_input_port_data *data)
 {
-	if (data->port.raw_data)
-		*(s32 *)data->port.raw_data = data->pin6_mv;
+	s32 new_value = data->pin6_mv;
+	s32 *raw_data = (s32 *)data->port.raw_data;
+
+	if (raw_data) {
+		if (*raw_data != new_value && data->port.last_changed_time)
+			*data->port.last_changed_time = ktime_get();
+
+		*raw_data = new_value;
+	}
 	if (data->port.notify_raw_data_func)
 		data->port.notify_raw_data_func(data->port.notify_raw_data_context);
 }
diff --git a/include/lego_port_class.h b/include/lego_port_class.h
@@ -84,6 +84,7 @@ struct lego_port_ev3_uart_ops {
  * @dev: The device data structure.
  * @raw_data: Pointer to raw data storage.
  * @raw_data_size: Size of raw_data in bytes.
+ * @last_changed_time: Time at which the raw_data last changed its contents.
  * @notify_raw_data_func: Registered by sensor drivers to be notified of new
  * 	raw data.
  * @notify_raw_data_context: Send to notify_raw_data_func as parameter.
@@ -112,6 +113,7 @@ struct lego_port_device {
 	struct device dev;
 	u8 *raw_data;
 	unsigned raw_data_size;
+	ktime_t *last_changed_time;
 	lego_port_notify_raw_data_func_t notify_raw_data_func;
 	void *notify_raw_data_context;
 };
@@ -126,11 +128,13 @@ extern void lego_port_unregister(struct lego_port_device *lego_port);
 static inline void
 lego_port_set_raw_data_ptr_and_func(struct lego_port_device *port,
 				    u8 *raw_data, unsigned raw_data_size,
+				    ktime_t *last_changed_time,
 				    lego_port_notify_raw_data_func_t func,
 				    void *context)
 {
 	port->raw_data = raw_data;
 	port->raw_data_size = raw_data_size;
+	port->last_changed_time = last_changed_time;
 	port->notify_raw_data_func = func;
 	port->notify_raw_data_context = context;
 }
diff --git a/include/lego_sensor_class.h b/include/lego_sensor_class.h
@@ -19,6 +19,7 @@
 
 #include <linux/device.h>
 #include <linux/types.h>
+#include <linux/ktime.h>
 
 #define LEGO_SENSOR_NAME_SIZE		30
 #define LEGO_SENSOR_FW_VERSION_SIZE	8
@@ -62,6 +63,7 @@ extern size_t lego_sensor_data_size[];
  * @figures: Number of digits that should be displayed, including decimal point.
  * @decimals: Decimal point position.
  * @raw_data: Raw data read from the sensor.
+ * @last_changed_time: Time at which the raw_data last changed its contents.
  */
 struct lego_sensor_mode_info {
 	char name[LEGO_SENSOR_MODE_NAME_SIZE + 1];
@@ -80,6 +82,7 @@ struct lego_sensor_mode_info {
 	u8 figures;
 	u8 decimals;
 	u8 raw_data[LEGO_SENSOR_RAW_DATA_SIZE];
+	ktime_t last_changed_time;
 };
 
 /**
diff --git a/pistorms/pistorms_ports_in.c b/pistorms/pistorms_ports_in.c
@@ -230,10 +230,21 @@ static void pistorms_poll_work(struct work_struct *work)
 		container_of(work, struct pistorms_in_port_data, poll_work);
 	u8 *raw_data = in_port->port.raw_data;
 	int ret;
+	u32 old_data[32];
+	int check_size = 0;
 
 	if (!raw_data)
 		return;
 
+	if (in_port->port.last_changed_time) {
+		check_size = in_port->port.raw_data_size <= 32
+				 ? in_port->port.raw_data_size : 32;
+	}
+
+	if (check_size) {
+		memcpy(old_data, raw_data, check_size);
+	}
+
 	switch (in_port->port.mode) {
 	case PS_IN_PORT_MODE_NXT_ANALOG:
 		ret = i2c_smbus_read_word_data(in_port->client,
@@ -277,6 +288,11 @@ static void pistorms_poll_work(struct work_struct *work)
 	}
 
 	lego_port_call_raw_data_func(&in_port->port);
+
+	if (check_size) {
+		if (memcmp(old_data, raw_data, check_size) != 0)
+			*in_port->port.last_changed_time = ktime_get();
+	}
 }
 
 enum hrtimer_restart pistorms_poll_timer_function(struct hrtimer *timer)
diff --git a/sensors/ev3_analog_sensor_core.c b/sensors/ev3_analog_sensor_core.c
@@ -61,7 +61,8 @@ static int ev3_analog_sensor_set_mode(void *context, u8 mode)
 	else
 		context = NULL;
 	lego_port_set_raw_data_ptr_and_func(data->ldev->port, mode_info->raw_data,
-		lego_sensor_get_raw_data_size(mode_info), func, context);
+		lego_sensor_get_raw_data_size(mode_info),
+		&mode_info->last_changed_time, func, context);
 
 	return 0;
 }
@@ -108,7 +109,8 @@ static int ev3_analog_sensor_remove(struct lego_device *ldev)
 {
 	struct ev3_analog_sensor_data *data = dev_get_drvdata(&ldev->dev);
 
-	lego_port_set_raw_data_ptr_and_func(ldev->port, NULL, 0, NULL, NULL);
+	lego_port_set_raw_data_ptr_and_func(ldev->port, NULL, 0, NULL,
+					    NULL, NULL);
 	unregister_lego_sensor(&data->sensor);
 	dev_set_drvdata(&ldev->dev, NULL);
 	kfree(data);
diff --git a/sensors/ev3_uart_sensor_core.c b/sensors/ev3_uart_sensor_core.c
@@ -59,7 +59,8 @@ static int ev3_uart_sensor_set_mode(void *context, u8 mode)
 		return -EOPNOTSUPP;
 
 	lego_port_set_raw_data_ptr_and_func(data->ldev->port, mode_info->raw_data,
-		lego_sensor_get_raw_data_size(mode_info), NULL, NULL);
+		lego_sensor_get_raw_data_size(mode_info),
+		&mode_info->last_changed_time, NULL, NULL);
 
 	return 0;
 }
@@ -112,7 +113,8 @@ static int ev3_uart_sensor_remove(struct lego_device *ldev)
 {
 	struct ev3_uart_sensor_data *data = dev_get_drvdata(&ldev->dev);
 
-	lego_port_set_raw_data_ptr_and_func(ldev->port, NULL, 0, NULL, NULL);
+	lego_port_set_raw_data_ptr_and_func(ldev->port, NULL, 0, NULL,
+					    NULL, NULL);
 	unregister_lego_sensor(&data->sensor);
 	dev_set_drvdata(&ldev->dev, NULL);
 	kfree(data);
diff --git a/sensors/ev3_uart_sensor_ld.c b/sensors/ev3_uart_sensor_ld.c
@@ -953,7 +953,16 @@ static int ev3_uart_receive_buf2(struct tty_struct *tty,
 			if (!completion_done(&port->set_mode_completion)
 			    && mode == port->new_mode)
 				complete(&port->set_mode_completion);
-			memcpy(port->mode_info[mode].raw_data, message + 1, msg_size - 2);
+
+			if (memcmp(port->mode_info[mode].raw_data, message + 1,
+				   msg_size - 2) != 0) {
+				port->mode_info[mode].last_changed_time =
+					ktime_get();
+				memcpy(port->mode_info[mode].raw_data,
+				       message + 1,
+				       msg_size - 2);
+			}
+
 			port->data_rec = 1;
 			if (port->num_data_err)
 				port->num_data_err--;
diff --git a/sensors/ht_nxt_smux.c b/sensors/ht_nxt_smux.c
@@ -469,16 +469,36 @@ void ht_nxt_smux_poll_cb(struct nxt_i2c_sensor_data *data)
 	struct ht_nxt_smux_port_data *ports = data->callback_data;
 	int i;
 	u8 raw_analog[2];
+	u8 old_data[32];
+	int raw_data_size;
+	int check_size;
+
+	raw_data_size = lego_sensor_get_raw_data_size(mode_info);
+	check_size = raw_data_size <= 32 ? raw_data_size : 32;
+	memcpy(old_data, mode_info->raw_data, raw_data_size);
 
 	i2c_smbus_read_i2c_block_data(data->client, i2c_info->read_data_reg,
-		lego_sensor_get_raw_data_size(mode_info), mode_info->raw_data);
+		raw_data_size, mode_info->raw_data);
+
+	if (memcmp(old_data, mode_info->raw_data, check_size) != 0)
+		mode_info->last_changed_time = ktime_get();
 
 	for (i = 0; i < NUM_HT_NXT_SMUX_CH; i++) {
+		ktime_t *last_change_ptr = ports[i].port.last_changed_time;
 		u8 *raw_data = ports[i].port.raw_data;
-		int raw_data_size = ports[i].port.raw_data_size;
+		raw_data_size = ports[i].port.raw_data_size;
 
 		if (!raw_data)
 			continue;
+
+		if (last_change_ptr)
+			check_size = raw_data_size <= 32 ? raw_data_size : 32;
+		else
+			check_size = 0;
+
+		if (check_size)
+			memcpy(old_data, raw_data, check_size);
+
 		if (ports[i].port.mode == HT_NXT_SMUX_PORT_MODE_ANALOG) {
 			i2c_smbus_read_i2c_block_data(data->client,
 				ht_nxt_smux_analog_data_reg[i], 2, raw_analog);
@@ -490,6 +510,12 @@ void ht_nxt_smux_poll_cb(struct nxt_i2c_sensor_data *data)
 				ht_nxt_smux_i2c_data_reg[i],
 				raw_data_size, raw_data);
 		}
+
+		if (check_size) {
+			if (memcmp(old_data, raw_data, check_size) != 0)
+				*last_change_ptr = ktime_get();
+		}
+
 		lego_port_call_raw_data_func(&ports[i].port);
 	}
 }
diff --git a/sensors/ht_nxt_smux_i2c_sensor.c b/sensors/ht_nxt_smux_i2c_sensor.c
@@ -57,6 +57,7 @@ static int ht_nxt_smux_i2c_sensor_set_mode(void *context, u8 mode)
 	ht_nxt_smux_port_set_i2c_data_reg(port, i2c_mode_info[mode].read_data_reg,
 					  size);
 	lego_port_set_raw_data_ptr_and_func(port, mode_info->raw_data, size,
+					    &mode_info->last_changed_time,
 					    NULL, NULL);
 
 	return 0;
@@ -155,7 +156,8 @@ static int ht_nxt_smux_i2c_sensor_remove(struct lego_device *ldev)
 {
 	struct ht_nxt_smux_i2c_sensor_data *data = dev_get_drvdata(&ldev->dev);
 
-	lego_port_set_raw_data_ptr_and_func(ldev->port, NULL, 0, NULL, NULL);
+	lego_port_set_raw_data_ptr_and_func(ldev->port, NULL, 0, NULL,
+					    NULL, NULL);
 	ldev->port->nxt_i2c_ops->set_pin1_gpio(ldev->port->context,
 					       LEGO_PORT_GPIO_FLOAT);
 	unregister_lego_sensor(&data->sensor);
diff --git a/sensors/lego_sensor_class.c b/sensors/lego_sensor_class.c
diff --git a/sensors/ms_ev3_smux.c b/sensors/ms_ev3_smux.c
diff --git a/sensors/nxt_analog_sensor_core.c b/sensors/nxt_analog_sensor_core.c
diff --git a/sensors/nxt_i2c_sensor_core.c b/sensors/nxt_i2c_sensor_core.c
diff --git a/wedo/wedo_hub.c b/wedo/wedo_hub.c
diff --git a/wedo/wedo_port.c b/wedo/wedo_port.c
