From f9c4a1048e515c49b8209862e7563d4ae9bb593f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jakub=20Van=C4=9Bk?= Date: Sun, 9 Aug 2020 17:25:41 +0200 Subject: [PATCH] sensors: add bin_data_age attribute --- brickpi/brickpi_i2c_sensor.c | 4 +++- brickpi/brickpi_serdev.c | 22 +++++++++++++++---- brickpi3/brickpi3_ports_in.c | 16 ++++++++++++++ ev3/ev3_ports_in.c | 22 +++++++++++++++---- include/lego_port_class.h | 4 ++++ include/lego_sensor_class.h | 3 +++ pistorms/pistorms_ports_in.c | 16 ++++++++++++++ sensors/ev3_analog_sensor_core.c | 6 ++++-- sensors/ev3_uart_sensor_core.c | 6 ++++-- sensors/ev3_uart_sensor_ld.c | 11 +++++++++- sensors/ht_nxt_smux.c | 30 ++++++++++++++++++++++++-- sensors/ht_nxt_smux_i2c_sensor.c | 4 +++- sensors/lego_sensor_class.c | 36 +++++++++++++++++++++++++++++--- sensors/ms_ev3_smux.c | 19 +++++++++++++++-- sensors/nxt_analog_sensor_core.c | 6 ++++-- sensors/nxt_i2c_sensor_core.c | 19 ++++++++++++++--- wedo/wedo_hub.c | 16 +++++++++++--- wedo/wedo_port.c | 7 ++++++- 18 files changed, 216 insertions(+), 31 deletions(-) diff --git a/brickpi/brickpi_i2c_sensor.c b/brickpi/brickpi_i2c_sensor.c index 58159251..1000723d 100644 --- 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 index 40f73348..cb55dc19 100644 --- 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 index c3c50542..287d8a55 100644 --- 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 index 29bf46e6..c1425225 100644 --- 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 index 5eae8908..1410749b 100644 --- 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 index 09cb8044..6fce3d9b 100644 --- a/include/lego_sensor_class.h +++ b/include/lego_sensor_class.h @@ -19,6 +19,7 @@ #include #include +#include #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 index 1743fcbc..6309b76e 100644 --- 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 index f9e38c8c..84beddcc 100644 --- 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 index 02320d8d..bcf0c7f0 100644 --- 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 index 0988f489..d2485e3e 100644 --- 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 index 7ce5dee8..55e80c56 100644 --- 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 index 1c3303a7..cd3aab14 100644 --- 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 index 888a9899..8a0e2246 100644 --- a/sensors/lego_sensor_class.c +++ b/sensors/lego_sensor_class.c @@ -63,6 +63,11 @@ * attributes. Use ``bin_data_format``, ``num_values`` and the individual * sensor documentation to determine how to interpret the data. * + * * - ``bin_data_age`` + * - read-only + * - Returns the number of milliseconds elapsed since the last change + * of ``bin_data``. + * * * - ``bin_data_format`` * - read-only * - Returns the format of the values in ``bin_data`` for the current @@ -298,13 +303,18 @@ static ssize_t mode_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) { struct lego_sensor_device *sensor = to_lego_sensor_device(dev); + struct lego_sensor_mode_info *mode_info; int i, err; for (i = 0; i < sensor->num_modes; i++) { - if (sysfs_streq(buf, sensor->mode_info[i].name)) { + mode_info = &sensor->mode_info[i]; + if (sysfs_streq(buf, mode_info->name)) { err = sensor->set_mode(sensor->context, i); if (err) return err; + if (mode_info->last_changed_time == 0) { + mode_info->last_changed_time = ktime_get(); + } if (sensor->mode != i) { sensor->mode = i; kobject_uevent(&dev->kobj, KOBJ_CHANGE); @@ -503,6 +513,24 @@ static ssize_t bin_data_format_show(struct device *dev, return sprintf(buf, "%s\n", value); } +static ssize_t bin_data_age_show(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct lego_sensor_device *sensor = to_lego_sensor_device(dev); + ktime_t last_change = sensor->mode_info[sensor->mode].last_changed_time; + struct timespec64 elapsed; + s64 msec; + + if (last_change == 0) + return sprintf(buf, "0\n"); + + elapsed = ktime_to_timespec64(ktime_sub(ktime_get(), last_change)); + msec = elapsed.tv_sec * 1000 + elapsed.tv_nsec / 1000000; + + return sprintf(buf, "%lld\n", msec); +} + static ssize_t poll_ms_show(struct device *dev, struct device_attribute *attr, char *buf) { @@ -553,10 +581,10 @@ static ssize_t text_value_show(struct device *dev, struct device_attribute *attr { struct lego_sensor_device *sensor = to_lego_sensor_device(dev); const char *value; - + if (!sensor->get_text_value) return -EOPNOTSUPP; - + value = sensor->get_text_value(sensor->context); if(IS_ERR(value)) @@ -622,6 +650,7 @@ static DEVICE_ATTR_RO(units); static DEVICE_ATTR_RO(decimals); static DEVICE_ATTR_RO(num_values); static DEVICE_ATTR_RO(bin_data_format); +static DEVICE_ATTR_RO(bin_data_age); static DEVICE_ATTR_RO(text_value); /* * Technically, it is possible to have 32 8-bit values from UART sensors @@ -652,6 +681,7 @@ static struct attribute *lego_sensor_class_attrs[] = { &dev_attr_decimals.attr, &dev_attr_num_values.attr, &dev_attr_bin_data_format.attr, + &dev_attr_bin_data_age.attr, &dev_attr_text_value.attr, &dev_attr_value0.attr, &dev_attr_value1.attr, diff --git a/sensors/ms_ev3_smux.c b/sensors/ms_ev3_smux.c index 01e6ae76..41992f40 100644 --- a/sensors/ms_ev3_smux.c +++ b/sensors/ms_ev3_smux.c @@ -187,13 +187,28 @@ static int ms_ev3_smux_set_mode(void *context, u8 mode) void ms_ev3_smux_poll_cb(struct nxt_i2c_sensor_data *data) { struct ms_ev3_smux_data *smux = data->callback_data; + u8 *raw_data = smux->port.raw_data; + unsigned int raw_data_size = smux->port.raw_data_size; + u8 old_data[32]; + int check_size = 0; - if (!smux->sensor || !smux->port.raw_data) + if (!smux->sensor || !raw_data) return; + if (smux->port.last_changed_time) + check_size = raw_data_size <= 32 ? raw_data_size : 32; + + if (check_size) + memcpy(old_data, smux->port.raw_data, check_size); + i2c_smbus_read_i2c_block_data(data->client, MS_EV3_SMUX_DATA_REG, - smux->port.raw_data_size, smux->port.raw_data); + raw_data_size, raw_data); lego_port_call_raw_data_func(&smux->port); + + if (check_size) { + if (memcmp(old_data, raw_data, check_size) != 0) + *smux->port.last_changed_time = ktime_get(); + } } static struct lego_port_ev3_analog_ops ms_ev3_smux_ev3_analog_ops = { diff --git a/sensors/nxt_analog_sensor_core.c b/sensors/nxt_analog_sensor_core.c index ff0ae417..03bc5316 100644 --- a/sensors/nxt_analog_sensor_core.c +++ b/sensors/nxt_analog_sensor_core.c @@ -63,7 +63,8 @@ static int nxt_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); data->ldev->port->nxt_analog_ops->set_pin5_gpio(data->ldev->port->context, data->info.analog_mode_info[mode].pin5_state); @@ -116,7 +117,8 @@ static int nxt_analog_sensor_remove(struct lego_device *ldev) ldev->port->nxt_analog_ops->set_pin5_gpio(ldev->port->context, LEGO_PORT_GPIO_FLOAT); - 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/nxt_i2c_sensor_core.c b/sensors/nxt_i2c_sensor_core.c index cfb381c4..d6dede2b 100644 --- a/sensors/nxt_i2c_sensor_core.c +++ b/sensors/nxt_i2c_sensor_core.c @@ -198,13 +198,26 @@ void nxt_i2c_sensor_poll_work(struct work_struct *work) struct lego_sensor_mode_info *mode_info = &data->sensor.mode_info[data->sensor.mode]; - if (data->info->ops && data->info->ops->poll_cb) + u8 old_data[32]; + int raw_data_size = lego_sensor_get_raw_data_size(mode_info); + int check_size = raw_data_size <= 32 ? raw_data_size : 32; + + + if (data->info->ops && data->info->ops->poll_cb) { data->info->ops->poll_cb(data); - else + } else { + + memcpy(old_data, mode_info->raw_data, check_size); + i2c_smbus_read_i2c_block_data(data->client, i2c_mode_info->read_data_reg, - lego_sensor_get_raw_data_size(mode_info), + 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(); + } + } } static int nxt_i2c_sensor_probe(struct i2c_client *client, diff --git a/wedo/wedo_hub.c b/wedo/wedo_hub.c index fdf70937..ef79d4ee 100644 --- a/wedo/wedo_hub.c +++ b/wedo/wedo_hub.c @@ -224,7 +224,9 @@ static void wedo_in_callback(struct urb *urb) struct lego_sensor_device *hub = &wedo->wedo_hub; struct wedo_port_data *wpd1 = wedo->wedo_ports[WEDO_PORT_1]; struct wedo_port_data *wpd2 = wedo->wedo_ports[WEDO_PORT_2]; - u16 *hub_raw_data = (u16 *)hub->mode_info[hub->mode].raw_data; + struct lego_sensor_mode_info *mode_info = &hub->mode_info[hub->mode]; + u16 *hub_raw_data = (u16 *)mode_info->raw_data; + ktime_t *hub_last_changed = &mode_info->last_changed_time; unsigned long flags; if (status) { @@ -238,14 +240,22 @@ static void wedo_in_callback(struct urb *urb) * is the only reader, and each byte of data is atomic. */ if (urb->actual_length == 8) { + u16 new_data[2]; + if (wedo->in_buf[0] & WEDO_HUB_CTL_BIT_ECHO) wedo->output_bits &= ~WEDO_HUB_CTL_BIT_ECHO; else wedo->output_bits |= WEDO_HUB_CTL_BIT_ECHO; - hub_raw_data[0] = wedo->in_buf[0]; + new_data[0] = wedo->in_buf[0]; /* multiplying by 49 scales the raw value to millivolts */ - hub_raw_data[1] = wedo->in_buf[1] * 49; + new_data[1] = wedo->in_buf[1] * 49; + + if (memcmp(hub_raw_data, new_data, 4) != 0) { + *hub_last_changed = ktime_get(); + memcpy(hub_raw_data, new_data, 4); + } + wpd1->input = wedo->in_buf[2]; wpd1->id = wedo->in_buf[3]; /* WEDO_HUB_CTL_BIT_ERROR indicates that outputs are turned off */ diff --git a/wedo/wedo_port.c b/wedo/wedo_port.c index 6653ba67..7f23428e 100644 --- a/wedo/wedo_port.c +++ b/wedo/wedo_port.c @@ -538,6 +538,7 @@ void wedo_port_update_status(struct wedo_port_data *wpd) struct wedo_sensor_data *wsd = NULL; struct wedo_motor_data *wmd = NULL; struct wedo_servo_data *wvd = NULL; + struct lego_sensor_mode_info *mode_info; switch (wpd->type_id) { @@ -545,7 +546,11 @@ void wedo_port_update_status(struct wedo_port_data *wpd) case WEDO_TYPE_MOTION: wsd = wpd->sensor_data; if (wsd) { - wsd->info.mode_info[wsd->sensor.mode].raw_data[0] = wpd->input; + mode_info = &wsd->info.mode_info[wsd->sensor.mode]; + + if (mode_info->raw_data[0] != wpd->input) + mode_info->last_changed_time = ktime_get(); + mode_info->raw_data[0] = wpd->input; } break; case WEDO_TYPE_SERVO: