Permalink
Browse files

legoev3-uart: got DATA messages reading and added registration of leg…

…oev3_port_device for UART sensor
  • Loading branch information...
1 parent 6adc03f commit 0fe200cb004c9fd3728461c0bca285405af5e089 @dlech dlech committed Jan 21, 2014
View
42 drivers/legoev3/ev3_input_port.c
@@ -193,91 +193,91 @@ enum sensor_type {
NUM_SENSOR
};
-const struct attribute_group *common_sensor_device_type_attr_groups[] = {
+const struct attribute_group *ev3_sensor_device_type_attr_groups[] = {
&legoev3_port_device_type_attr_grp,
NULL
};
struct device_type ev3_sensor_device_types[] = {
[SENSOR_NXT_TOUCH] = {
.name = "nxt-touch-sensor",
- .groups = common_sensor_device_type_attr_groups,
+ .groups = ev3_sensor_device_type_attr_groups,
},
[SENSOR_NXT_LIGHT] = {
.name = "nxt-light-sensor",
- .groups = common_sensor_device_type_attr_groups,
+ .groups = ev3_sensor_device_type_attr_groups,
},
[SENSOR_NXT_COLOR] = {
.name = "nxt-color-sensor",
- .groups = common_sensor_device_type_attr_groups,
+ .groups = ev3_sensor_device_type_attr_groups,
},
[SENSOR_NXT_DUMB] = {
.name = "nxt-generic-analog-sensor",
- .groups = common_sensor_device_type_attr_groups,
+ .groups = ev3_sensor_device_type_attr_groups,
},
[SENSOR_NXT_I2C] = {
.name = "nxt-i2c-sensor",
- .groups = common_sensor_device_type_attr_groups,
+ .groups = ev3_sensor_device_type_attr_groups,
},
[SENSOR_EV3_ID_01] = {
.name = "ev3-sensor-01",
- .groups = common_sensor_device_type_attr_groups,
+ .groups = ev3_sensor_device_type_attr_groups,
},
[SENSOR_EV3_ID_02] = {
.name = "ev3-touch-sensor",
- .groups = common_sensor_device_type_attr_groups,
+ .groups = ev3_sensor_device_type_attr_groups,
},
[SENSOR_EV3_ID_03] = {
.name = "ev3-sensor-03",
- .groups = common_sensor_device_type_attr_groups,
+ .groups = ev3_sensor_device_type_attr_groups,
},
[SENSOR_EV3_ID_04] = {
.name = "ev3-sensor-04",
- .groups = common_sensor_device_type_attr_groups,
+ .groups = ev3_sensor_device_type_attr_groups,
},
[SENSOR_EV3_ID_05] = {
.name = "ev3-sensor-05",
- .groups = common_sensor_device_type_attr_groups,
+ .groups = ev3_sensor_device_type_attr_groups,
},
[SENSOR_EV3_ID_06] = {
.name = "ev3-sensor-06",
- .groups = common_sensor_device_type_attr_groups,
+ .groups = ev3_sensor_device_type_attr_groups,
},
[SENSOR_EV3_ID_07] = {
.name = "ev3-sensor-07",
- .groups = common_sensor_device_type_attr_groups,
+ .groups = ev3_sensor_device_type_attr_groups,
},
[SENSOR_EV3_ID_08] = {
.name = "ev3-sensor-08",
- .groups = common_sensor_device_type_attr_groups,
+ .groups = ev3_sensor_device_type_attr_groups,
},
[SENSOR_EV3_ID_09] = {
.name = "ev3-sensor-09",
- .groups = common_sensor_device_type_attr_groups,
+ .groups = ev3_sensor_device_type_attr_groups,
},
[SENSOR_EV3_ID_10] = {
.name = "ev3-sensor-10",
- .groups = common_sensor_device_type_attr_groups,
+ .groups = ev3_sensor_device_type_attr_groups,
},
[SENSOR_EV3_ID_11] = {
.name = "ev3-sensor-11",
- .groups = common_sensor_device_type_attr_groups,
+ .groups = ev3_sensor_device_type_attr_groups,
},
[SENSOR_EV3_ID_12] = {
.name = "ev3-sensor-12",
- .groups = common_sensor_device_type_attr_groups,
+ .groups = ev3_sensor_device_type_attr_groups,
},
[SENSOR_EV3_ID_13] = {
.name = "ev3-sensor-13",
- .groups = common_sensor_device_type_attr_groups,
+ .groups = ev3_sensor_device_type_attr_groups,
},
[SENSOR_EV3_ID_14] = {
.name = "ev3-sensor-14",
- .groups = common_sensor_device_type_attr_groups,
+ .groups = ev3_sensor_device_type_attr_groups,
},
[SENSOR_EV3_UART] = {
.name = "ev3-uart-sensor",
- .groups = common_sensor_device_type_attr_groups,
+ .groups = ev3_sensor_device_type_attr_groups,
},
};
View
1 drivers/legoev3/ev3_uart_sensor.c
@@ -76,7 +76,6 @@ struct legoev3_port_driver ev3_uart_sensor_driver = {
.owner = THIS_MODULE,
},
};
-EXPORT_SYMBOL_GPL(ev3_uart_sensor_driver);
legoev3_port_driver(ev3_uart_sensor_driver);
MODULE_DESCRIPTION("EV3 UART sensor device driver for LEGO Mindstorms EV3");
View
483 drivers/legoev3/legoev3_uart.c
@@ -14,25 +14,42 @@
*/
#include <linux/bitops.h>
+#include <linux/jiffies.h>
#include <linux/module.h>
#include <linux/slab.h>
#include <linux/string.h>
#include <linux/tty.h>
-
-
-#define BUFFER_SIZE 50
-#define NAME_SIZE 11
-#define UNITS_SIZE 4
-#define SENOR_DATA_SIZE 32
-
-#define LEGOEV3_UART_MSG_TYPE_MASK 0xC0
-#define LEGOEV3_UART_CMD_SIZE(byte) (1 << ((byte >> 3) & 0x7))
-#define LEGOEV3_UART_MSG_CMD_MASK 0x07
-#define LEGOEV3_UART_TYPE_MAX 101
-#define LEGOEV3_UART_MODE_MAX 7
-
-#define LEGOEV3_UART_TYPE_UNKNOWN 125
-#define LEGOEV3_UART_SPEED_MIN 2400
+#include <linux/legoev3/legoev3_ports.h>
+#include <linux/legoev3/legoev3_uart.h>
+
+#ifdef DEBUG
+#define debug_pr(fmt, ...) printk(fmt, ##__VA_ARGS__)
+#else
+#define debug_pr(fmt, ...) while(0) { }
+#endif
+
+#define LEGOEV3_UART_BUFFER_SIZE 50
+#define LEGOEV3_UART_NAME_SIZE 11
+#define LEGOEV3_UART_UNITS_SIZE 4
+#define LEGOEV3_UART_SENSOR_DATA_SIZE 32
+
+#define LEGOEV3_UART_MSG_TYPE_MASK 0xC0
+#define LEGOEV3_UART_CMD_SIZE(byte) (1 << ((byte >> 3) & 0x7))
+#define LEGOEV3_UART_MSG_CMD_MASK 0x07
+#define LEGOEV3_UART_TYPE_MAX 101
+#define LEGOEV3_UART_MODE_MAX 7
+#define LEGOEV3_UART_MAX_DATA_ERR 6
+
+#define LEGOEV3_UART_TYPE_UNKNOWN 125
+#define LEGOEV3_UART_SPEED_MIN 2400
+#define LEGOEV3_UART_SPEED_MID 57600
+#define LEGOEV3_UART_SPEED_MAX 460800
+
+#define LEGOEV3_UART_SEND_ACK_DELAY 10 /* msec */
+#define LEGOEV3_UART_SET_BITRATE_DELAY 10 /* msec */
+#define LEGOEV3_UART_DATA_WATCHDOG_TIMEOUT 100 /* msec */
+
+#define LEGOEV3_UART_DEVICE_TYPE_NAME_SIZE 30
enum legoev3_uart_msg_type {
LEGOEV3_UART_MSG_TYPE_SYS = 0x00,
@@ -85,7 +102,7 @@ enum legoev3_uart_info_flags {
LEGOEV3_UART_INFO_FLAG_INFO_SI = BIT(LEGOEV3_UART_INFO_BIT_INFO_SI),
LEGOEV3_UART_INFO_FLAG_INFO_UNITS = BIT(LEGOEV3_UART_INFO_BIT_INFO_UNITS),
LEGOEV3_UART_INFO_FLAG_INFO_FORMAT = BIT(LEGOEV3_UART_INFO_BIT_INFO_FORMAT),
- LEGOEV3_UART_INFO_FLAG_INFO_ALL = LEGOEV3_UART_INFO_FLAG_INFO_NAME
+ LEGOEV3_UART_INFO_FLAG_ALL_INFO = LEGOEV3_UART_INFO_FLAG_INFO_NAME
| LEGOEV3_UART_INFO_FLAG_INFO_RAW
| LEGOEV3_UART_INFO_FLAG_INFO_PCT
| LEGOEV3_UART_INFO_FLAG_INFO_SI
@@ -97,45 +114,103 @@ enum legoev3_uart_info_flags {
| LEGOEV3_UART_INFO_FLAG_INFO_FORMAT,
};
-struct legoev3_uart_sensor_info {
- u8 type;
+enum legoev3_uart_data_type {
+ LEGOEV3_UART_DATA_8,
+ LEGOEV3_UART_DATA_16,
+ LEGOEV3_UART_DATA_32,
+ LEGOEV3_UART_DATA_FLOAT,
+};
+
+static char legoev3_uart_sensor_type_names[LEGOEV3_UART_TYPE_MAX + 1][LEGOEV3_UART_DEVICE_TYPE_NAME_SIZE];
+static struct device_type legoev3_uart_sensor_device_types[LEGOEV3_UART_TYPE_MAX + 1];
+
+/**
+ * struct legoev3_uart_sensor_mode_info
+ * @mode: The id of this mode. (0-7)
+ * @name: The name of this mode
+ * @raw_min: The minimum raw value of the data read.
+ * @raw_min: The maximum raw value of the data read.
+ * @pct_min: The minimum percentage value of the data read.
+ * @pct_min: The maximum percentage value of the data read.
+ * @si_min: The minimum scaled value of the data read.
+ * @si_min: The maximum scaled value of the data read.
+ * @units: Units of the scaled value.
+ * @data_sets: Number of data points in DATA message.
+ * @format: Format of data in DATA message.
+ * @figures: Number of digits that should be displayed, including decimal point.
+ * @decimals: Decimal point position.
+ * @raw_data: Raw data read from the sensor.
+ *
+ * All of the min an max values are actually 32-bit float data type,
+ * but kernel drivers don't do floating point, so we use unsigned instead.
+ */
+struct legoev3_uart_mode_info {
u8 mode;
- u8 num_modes;
- unsigned speed;
- char name[NAME_SIZE + 1];
- /* min/max values are actually float data type */
+ char name[LEGOEV3_UART_NAME_SIZE + 1];
unsigned raw_min;
unsigned raw_max;
unsigned pct_min;
unsigned pct_max;
unsigned si_min;
unsigned si_max;
- char units[UNITS_SIZE + 1];
+ char units[LEGOEV3_UART_UNITS_SIZE + 1];
u8 data_sets;
- u8 format;
+ enum legoev3_uart_data_type format;
u8 figures;
u8 decimals;
+ u8 raw_data[32];
};
-static struct legoev3_uart_sensor_info default_sensor_info = {
- .type = LEGOEV3_UART_TYPE_UNKNOWN,
- .speed = LEGOEV3_UART_SPEED_MIN,
+static struct legoev3_uart_mode_info legoev3_uart_default_mode_info = {
.raw_max = 0x447fc000, /* 1023.0 */
.pct_max = 0x42c80000, /* 100.0 */
.si_max = 0x3f800000, /* 1.0 */
+ .figures = 4,
};
+/**
+ * struct legoev3_uart_data - Discipline data for EV3 UART Sensor communication
+ * @tty: Pointer to the tty device that the sensor is connected to
+ * @sensor: The real sensor device.
+ * @send_ack_work: Used to send ACK after a delay.
+ * @change_bitrate_work: Used to change the baud rate after a delay.
+ * @mode_info: Array of information about each mode of the sensor
+ * @type: The type of sensor that we are connected to. *
+ * @num_modes: The number of modes that the sensor has. (1-8)
+ * @num_view_modes: Number of modes that can be used for data logging. (1-8)
+ * @mode: The current mode.
+ * @info_flags: Flags indicating what information has already been read
+ * from the sensor.
+ * @buffer: Byte array to store received data in between receive_buf interrupts.
+ * @write_ptr: The current position in the buffer.
+ * @data_watchdog: Watchdog timer for receiving DATA messages.
+ * @num_data_err: Number of bad reads when receiving DATA messages.
+ * @synced: Flag indicating communications are synchronized with the sensor.
+ * @info_done: Flag indicating that all mode info has been received and it is
+ * OK to start receiving DATA messages.
+ * @data_rec: Flag that indicates that good DATA message has been received
+ * since last watchdog timeout.
+ */
struct legoev3_uart_data {
struct tty_struct *tty;
- struct legoev3_uart_sensor_info sensor_info[LEGOEV3_UART_MODE_MAX + 1];
+ struct legoev3_port_device *sensor;
+ struct legoev3_uart_sensor_platform_data pdata;
+ struct delayed_work send_ack_work;
+ struct delayed_work change_bitrate_work;
+ struct legoev3_uart_mode_info mode_info[LEGOEV3_UART_MODE_MAX + 1];
u8 type;
u8 num_modes;
u8 num_view_modes;
u8 mode;
+ speed_t new_baud_rate;
long unsigned info_flags;
- u8 buffer[BUFFER_SIZE];
+ u8 buffer[LEGOEV3_UART_BUFFER_SIZE];
unsigned write_ptr;
+ unsigned long data_watchdog;
+ unsigned num_data_err;
unsigned synced:1;
+ unsigned info_done:1;
+ unsigned data_rec:1;
};
static inline int legoev3_uart_msg_size(u8 header)
@@ -153,16 +228,76 @@ static inline int legoev3_uart_msg_size(u8 header)
return size;
}
+int legoev3_uart_write_byte(struct tty_struct *tty, const u8 byte)
+{
+ int ret;
+
+ set_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
+ while (!(ret = tty->ops->write(tty, &byte, 1)));
+
+ return ret < 0 ? ret : 0;
+}
+
+static void legoev3_uart_send_ack(struct work_struct *work)
+{
+ struct delayed_work *dwork = to_delayed_work(work);
+ struct legoev3_uart_data *data =
+ container_of(dwork, struct legoev3_uart_data, send_ack_work);
+ struct legoev3_port_device *sensor;
+
+ if (!data->sensor && data->type <= LEGOEV3_UART_TYPE_MAX) {
+ data->pdata.tty = data->tty;
+ data->pdata.mode_info = data->mode_info;
+ data->pdata.num_modes = data->num_modes;
+ data->pdata.num_view_modes = data->num_view_modes;
+ sensor = legoev3_port_device_register(
+ legoev3_uart_sensor_type_names[data->type],
+ -1, /* TODO: get input port ID here */
+ &legoev3_uart_sensor_device_types[data->type],
+ &data->pdata,
+ sizeof(struct legoev3_uart_sensor_platform_data),
+ data->tty->dev);
+ if (IS_ERR(sensor)) {
+ dev_err(data->tty->dev, "Could not register UART sensor on tty %s",
+ data->tty->name);
+ return;
+ }
+ data->sensor = sensor;
+ }
+
+ legoev3_uart_write_byte(data->tty, LEGOEV3_UART_SYS_ACK);
+ schedule_delayed_work(&data->change_bitrate_work,
+ msecs_to_jiffies(LEGOEV3_UART_SET_BITRATE_DELAY));
+}
+
+static void legoev3_uart_change_bitrate(struct work_struct *work)
+{
+ struct delayed_work *dwork = to_delayed_work(work);
+ struct legoev3_uart_data *data =
+ container_of(dwork, struct legoev3_uart_data, change_bitrate_work);
+ struct ktermios old_termios = *data->tty->termios;
+
+ mutex_lock(&data->tty->termios_mutex);
+ tty_encode_baud_rate(data->tty, data->new_baud_rate, data->new_baud_rate);
+ if (data->tty->ops->set_termios)
+ data->tty->ops->set_termios(data->tty, &old_termios);
+ mutex_unlock(&data->tty->termios_mutex);
+}
+
static int legoev3_uart_open(struct tty_struct *tty)
{
struct ktermios old_termios = *tty->termios;
struct legoev3_uart_data *data;
-printk("%s\n", __func__);
data = kzalloc(sizeof(struct legoev3_uart_data), GFP_KERNEL);
if (!data)
return -ENOMEM;
+ data->tty = tty;
+ data->type = LEGOEV3_UART_TYPE_UNKNOWN;
+ data->new_baud_rate = LEGOEV3_UART_SPEED_MIN;
+ INIT_DELAYED_WORK(&data->send_ack_work, legoev3_uart_send_ack);
+ INIT_DELAYED_WORK(&data->change_bitrate_work, legoev3_uart_change_bitrate);
tty->disc_data = data;
/* set baud rate and other port settings */
@@ -207,12 +342,19 @@ printk("%s\n", __func__);
static void legoev3_uart_close(struct tty_struct *tty)
{
-printk("%s\n", __func__);
+ struct legoev3_uart_data *data = tty->disc_data;
+
+ if (data->sensor)
+ legoev3_port_device_unregister(data->sensor);
+ cancel_delayed_work_sync(&data->send_ack_work);
+ cancel_delayed_work_sync(&data->change_bitrate_work);
+ tty->disc_data = NULL;
+ kfree(data);
}
static int legoev3_uart_ioctl(struct tty_struct *tty, struct file *file,
unsigned int cmd, unsigned long arg)
-{printk("%s\n", __func__);
+{
return tty_mode_ioctl(tty, file, cmd, arg);
}
@@ -225,11 +367,13 @@ static void legoev3_uart_receive_buf(struct tty_struct *tty,
int j, speed;
u8 cmd, cmd2, type, mode, msg_type, msg_size, chksum;
-printk("received: ");
-for (i = 0; i < count; i++)
- printk("0x%02x ", cp[i]);
-printk(" (%d)\n", count);
-i=0;
+#ifdef DEBUG
+ printk("received: ");
+ for (i = 0; i < count; i++)
+ printk("0x%02x ", cp[i]);
+ printk(" (%d)\n", count);
+ i=0;
+#endif
/*
* to get in sync with the data stream from the sensor, we look
@@ -247,26 +391,38 @@ i=0;
chksum = 0xFF ^ cmd ^ type;
if (cp[i+1] != chksum)
continue;
+ data->num_modes = 1;
+ data->num_view_modes = 1;
for (j = 0; j <= LEGOEV3_UART_MODE_MAX; j++)
- data->sensor_info[j] = default_sensor_info;
+ data->mode_info[j] = legoev3_uart_default_mode_info;
data->type = type;
- data->num_modes = 0;
- data->num_view_modes = 0;
data->info_flags = LEGOEV3_UART_INFO_FLAG_CMD_TYPE;
data->synced = 1;
+ data->info_done = 0;
data->write_ptr = 0;
i += 2;
}
if (!data->synced)
return;
+ if (data->info_done && time_is_before_jiffies(data->data_watchdog)) {
+ data->data_watchdog =
+ jiffies + msecs_to_jiffies(LEGOEV3_UART_DATA_WATCHDOG_TIMEOUT);
+ if (!data->data_rec) {
+ data->num_data_err++;
+
+ }
+ /* it appears that NACK is sent as a keep-alive */
+ legoev3_uart_write_byte(tty, LEGOEV3_UART_SYS_NACK);
+ data->data_rec = 0;
+ }
/*
* Once we are synced, we keep reading data until we have read
* a complete command.
*/
while (i < count) {
- if (data->write_ptr >= BUFFER_SIZE)
- goto err_bad_data;
+ if (data->write_ptr >= LEGOEV3_UART_BUFFER_SIZE)
+ goto err_invalid_state;
data->buffer[data->write_ptr++] = cp[i++];
}
@@ -276,11 +432,13 @@ i=0;
while ((msg_size = legoev3_uart_msg_size(data->buffer[0]))
<= data->write_ptr)
{
-printk("processing: ");
-for (i = 0; i < data->write_ptr; i++)
- printk("0x%02x ", data->buffer[i]);
-printk(" (%d)\n", data->write_ptr);
+#ifdef DEBUG
+ printk("processing: ");
+ for (i = 0; i < data->write_ptr; i++)
+ printk("0x%02x ", data->buffer[i]);
+ printk(" (%d)\n", data->write_ptr);
printk("msg_size:%d\n", msg_size);
+#endif
msg_type = data->buffer[0] & LEGOEV3_UART_MSG_TYPE_MASK;
cmd = data->buffer[0] & LEGOEV3_UART_MSG_CMD_MASK;
mode = cmd;
@@ -289,125 +447,206 @@ printk(" (%d)\n", data->write_ptr);
chksum = 0xFF;
for (i = 0; i < msg_size - 1; i++)
chksum ^= data->buffer[i];
- printk("chksum:%d, actual:%d\n", chksum, data->buffer[msg_size - 1]);
- if (chksum != data->buffer[msg_size - 1])
- goto err_bad_data;
+ debug_pr("chksum:%d, actual:%d\n",
+ chksum, data->buffer[msg_size - 1]);
+ if (chksum != data->buffer[msg_size - 1]) {
+ if (data->info_done) {
+ data->num_data_err++;
+ goto err_bad_data_msg_checksum;
+ }
+ else
+ goto err_invalid_state;
+ }
}
switch (msg_type) {
case LEGOEV3_UART_MSG_TYPE_SYS:
- printk("SYS:%d\n", data->buffer[0] & LEGOEV3_UART_MSG_CMD_MASK);
+ debug_pr("SYS:%d\n", data->buffer[0] & LEGOEV3_UART_MSG_CMD_MASK);
+ switch(cmd) {
+ case LEGOEV3_UART_SYS_SYNC:
+ if ((cmd ^ cmd2) == 0xFF)
+ msg_size++;
+ break;
+ case LEGOEV3_UART_SYS_ACK:
+ if (!data->num_modes)
+ /* we are still receiving mode data */
+ goto err_invalid_state;
+ if ((data->info_flags & LEGOEV3_UART_INFO_FLAG_REQUIRED)
+ != LEGOEV3_UART_INFO_FLAG_REQUIRED)
+ goto err_invalid_state;
+ schedule_delayed_work(&data->send_ack_work,
+ msecs_to_jiffies(LEGOEV3_UART_SEND_ACK_DELAY));
+ data->info_done = 1;
+ data->data_watchdog = jiffies
+ + msecs_to_jiffies(LEGOEV3_UART_SEND_ACK_DELAY
+ + LEGOEV3_UART_SET_BITRATE_DELAY);
+ data->num_data_err = 0;
+ break;
+ }
break;
case LEGOEV3_UART_MSG_TYPE_CMD:
- printk("CMD:%d\n", cmd);
- /* TODO: handle ACK */
+ debug_pr("CMD:%d\n", cmd);
switch (cmd) {
case LEGOEV3_UART_CMD_MODES:
- if (test_and_set_bit(LEGOEV3_UART_INFO_BIT_CMD_MODES, &data->info_flags))
- goto err_bad_data;
+ if (test_and_set_bit(LEGOEV3_UART_INFO_BIT_CMD_MODES,
+ &data->info_flags))
+ goto err_invalid_state;
if (!cmd2 || cmd2 > LEGOEV3_UART_MODE_MAX)
- goto err_bad_data;
+ goto err_invalid_state;
data->num_modes = cmd2 + 1;
if (msg_size > 3)
data->num_view_modes = data->buffer[2] + 1;
else
data->num_view_modes = data->num_modes;
- printk("num_modes:%d, num_view_modes:%d\n", data->num_modes, data->num_view_modes);
+ debug_pr("num_modes:%d, num_view_modes:%d\n",
+ data->num_modes, data->num_view_modes);
break;
case LEGOEV3_UART_CMD_SPEED:
- if (test_and_set_bit(LEGOEV3_UART_INFO_BIT_CMD_SPEED, &data->info_flags))
- goto err_bad_data;
+ if (test_and_set_bit(LEGOEV3_UART_INFO_BIT_CMD_SPEED,
+ &data->info_flags))
+ goto err_invalid_state;
speed = *(int*)(data->buffer + 1);
- /* TODO: change uart bit rate */
- printk("speed:%d\n", speed);
+ if (speed < LEGOEV3_UART_SPEED_MIN
+ || speed > LEGOEV3_UART_SPEED_MAX)
+ goto err_invalid_state;
+ data->new_baud_rate = speed;
+ debug_pr("speed:%d\n", speed);
break;
default:
- goto err_bad_data;
+ goto err_invalid_state;
}
break;
case LEGOEV3_UART_MSG_TYPE_INFO:
- printk("INFO:%d, mode:%d\n", cmd2, mode);
+ debug_pr("INFO:%d, mode:%d\n", cmd2, mode);
switch (cmd2) {
case LEGOEV3_UART_INFO_NAME:
- data->info_flags &= ~LEGOEV3_UART_INFO_FLAG_INFO_ALL;
+ data->info_flags &= ~LEGOEV3_UART_INFO_FLAG_ALL_INFO;
if (data->buffer[2] < 'A' || data->buffer[2] > 'z')
- goto err_bad_data;
- if (strlen(data->buffer + 2) > NAME_SIZE)
- goto err_bad_data;
- snprintf(data->sensor_info[mode].name,
- NAME_SIZE + 1, "%s", data->buffer + 2);
+ goto err_invalid_state;
+ /*
+ * Name may not have null terminator and we
+ * are done with the checksum at this point
+ * so we are writing 0 over the checksum to
+ * ensure a null terminator for the string
+ * functions.
+ */
+ data->buffer[msg_size - 1] = 0;
+ if (strlen(data->buffer + 2) > LEGOEV3_UART_NAME_SIZE)
+ goto err_invalid_state;
+ snprintf(data->mode_info[mode].name,
+ LEGOEV3_UART_NAME_SIZE + 1, "%s",
+ data->buffer + 2);
data->mode = mode;
data->info_flags |= LEGOEV3_UART_INFO_FLAG_INFO_NAME;
- printk("mode %d name:%s\n", mode, data->sensor_info[mode].name);
+ debug_pr("mode %d name:%s\n",
+ mode, data->mode_info[mode].name);
break;
case LEGOEV3_UART_INFO_RAW:
if (data->mode != mode)
- goto err_bad_data;
- if (test_and_set_bit(LEGOEV3_UART_INFO_BIT_INFO_RAW, &data->info_flags))
- goto err_bad_data;
- data->sensor_info[mode].raw_min = *(unsigned *)(data->buffer + 2);
- data->sensor_info[mode].raw_max = *(unsigned *)(data->buffer + 6);
- printk("mode %d raw_min:%08x, raw_max:%08x\n", mode, data->sensor_info[mode].raw_min, data->sensor_info[mode].raw_max);
+ goto err_invalid_state;
+ if (test_and_set_bit(LEGOEV3_UART_INFO_BIT_INFO_RAW,
+ &data->info_flags))
+ goto err_invalid_state;
+ data->mode_info[mode].raw_min =
+ *(unsigned *)(data->buffer + 2);
+ data->mode_info[mode].raw_max =
+ *(unsigned *)(data->buffer + 6);
+ debug_pr("mode %d raw_min:%08x, raw_max:%08x\n",
+ mode, data->mode_info[mode].raw_min,
+ data->mode_info[mode].raw_max);
break;
case LEGOEV3_UART_INFO_PCT:
if (data->mode != mode)
- goto err_bad_data;
- if (test_and_set_bit(LEGOEV3_UART_INFO_BIT_INFO_PCT, &data->info_flags))
- goto err_bad_data;
- data->sensor_info[mode].pct_min = *(unsigned *)(data->buffer + 2);
- data->sensor_info[mode].pct_max = *(unsigned *)(data->buffer + 6);
- printk("mode %d pct_min:%08x, pct_max:%08x\n", mode, data->sensor_info[mode].pct_min, data->sensor_info[mode].pct_max);
+ goto err_invalid_state;
+ if (test_and_set_bit(LEGOEV3_UART_INFO_BIT_INFO_PCT,
+ &data->info_flags))
+ goto err_invalid_state;
+ data->mode_info[mode].pct_min =
+ *(unsigned *)(data->buffer + 2);
+ data->mode_info[mode].pct_max =
+ *(unsigned *)(data->buffer + 6);
+ debug_pr("mode %d pct_min:%08x, pct_max:%08x\n",
+ mode, data->mode_info[mode].pct_min,
+ data->mode_info[mode].pct_max);
break;
case LEGOEV3_UART_INFO_SI:
if (data->mode != mode)
- goto err_bad_data;
- if (test_and_set_bit(LEGOEV3_UART_INFO_BIT_INFO_SI, &data->info_flags))
- goto err_bad_data;
- data->sensor_info[mode].si_min = *(unsigned *)(data->buffer + 2);
- data->sensor_info[mode].si_max = *(unsigned *)(data->buffer + 6);
- printk("mode %d si_min:%08x, si_max:%08x\n", mode, data->sensor_info[mode].si_min, data->sensor_info[mode].si_max);
+ goto err_invalid_state;
+ if (test_and_set_bit(LEGOEV3_UART_INFO_BIT_INFO_SI,
+ &data->info_flags))
+ goto err_invalid_state;
+ data->mode_info[mode].si_min =
+ *(unsigned *)(data->buffer + 2);
+ data->mode_info[mode].si_max =
+ *(unsigned *)(data->buffer + 6);
+ debug_pr("mode %d si_min:%08x, si_max:%08x\n",
+ mode, data->mode_info[mode].si_min,
+ data->mode_info[mode].si_max);
break;
case LEGOEV3_UART_INFO_UNITS:
if (data->mode != mode)
- goto err_bad_data;
- if (test_and_set_bit(LEGOEV3_UART_INFO_BIT_INFO_UNITS, &data->info_flags))
- goto err_bad_data;
- snprintf(data->sensor_info[mode].units,
- UNITS_SIZE + 1, "%s", data->buffer + 2);
- printk("mode %d units:%s\n", mode, data->sensor_info[mode].units);
+ goto err_invalid_state;
+ if (test_and_set_bit(LEGOEV3_UART_INFO_BIT_INFO_UNITS,
+ &data->info_flags))
+ goto err_invalid_state;
+ /*
+ * Units may not have null terminator and we
+ * are done with the checksum at this point
+ * so we are writing 0 over the checksum to
+ * ensure a null terminator for the string
+ * functions.
+ */
+ data->buffer[msg_size - 1] = 0;
+ snprintf(data->mode_info[mode].units,
+ LEGOEV3_UART_UNITS_SIZE + 1, "%s",
+ data->buffer + 2);
+ debug_pr("mode %d units:%s\n",
+ mode, data->mode_info[mode].units);
break;
case LEGOEV3_UART_INFO_FORMAT:
if (data->mode != mode)
- goto err_bad_data;
- if (test_and_set_bit(LEGOEV3_UART_INFO_BIT_INFO_FORMAT, &data->info_flags))
- goto err_bad_data;
- data->sensor_info[mode].data_sets = data->buffer[2];
- if (!data->sensor_info[mode].data_sets)
- goto err_bad_data;
+ goto err_invalid_state;
+ if (test_and_set_bit(LEGOEV3_UART_INFO_BIT_INFO_FORMAT,
+ &data->info_flags))
+ goto err_invalid_state;
+ data->mode_info[mode].data_sets = data->buffer[2];
+ if (!data->mode_info[mode].data_sets)
+ goto err_invalid_state;
if (msg_size < 7)
- goto err_bad_data;
+ goto err_invalid_state;
if ((data->info_flags & LEGOEV3_UART_INFO_FLAG_REQUIRED)
!= LEGOEV3_UART_INFO_FLAG_REQUIRED)
- goto err_bad_data;
- data->sensor_info[mode].format = data->buffer[3];
+ goto err_invalid_state;
+ data->mode_info[mode].format = data->buffer[3];
if (data->mode) {
data->mode--;
- data->sensor_info[mode].type = data->type;
- data->sensor_info[mode].num_modes = data->num_modes;
- data->sensor_info[mode].figures = data->buffer[4];
- data->sensor_info[mode].decimals = data->buffer[5];
+ data->mode_info[mode].figures = data->buffer[4];
+ data->mode_info[mode].decimals = data->buffer[5];
/* TODO: copy IR Seeker hack from lms2012 */
}
- printk("mode %d data_sets:%d, format:%d, figures:%d, decimals:%d\n",
- mode, data->sensor_info[mode].data_sets, data->sensor_info[mode].format,
- data->sensor_info[mode].figures, data->sensor_info[mode].decimals);
+ debug_pr("mode %d data_sets:%d, format:%d, figures:%d, decimals:%d\n",
+ mode, data->mode_info[mode].data_sets,
+ data->mode_info[mode].format,
+ data->mode_info[mode].figures,
+ data->mode_info[mode].decimals);
break;
}
break;
case LEGOEV3_UART_MSG_TYPE_DATA:
- printk("DATA:%d\n", data->buffer[0] & LEGOEV3_UART_MSG_CMD_MASK);
+ debug_pr("DATA:%d\n", data->buffer[0] & LEGOEV3_UART_MSG_CMD_MASK);
+ if (!data->info_done)
+ goto err_invalid_state;
+ memcpy(data->mode_info[mode].raw_data,
+ data->buffer + 1, msg_size - 2);
+ data->data_rec = 1;
+ if (data->num_data_err)
+ data->num_data_err--;
break;
}
+err_bad_data_msg_checksum:
+ if (data->num_data_err > LEGOEV3_UART_MAX_DATA_ERR)
+ goto err_invalid_state;
+
/*
* If there is leftover data, we move it to the beginning
* of the buffer.
@@ -418,15 +657,23 @@ printk(" (%d)\n", data->write_ptr);
}
return;
-err_bad_data:
+err_invalid_state:
data->synced = 0;
+ data->new_baud_rate = LEGOEV3_UART_SPEED_MIN;
+ schedule_delayed_work(&data->change_bitrate_work,
+ msecs_to_jiffies(LEGOEV3_UART_SET_BITRATE_DELAY));
}
static void legoev3_uart_write_wakeup(struct tty_struct *tty)
{
-printk("%s\n", __func__);
+ debug_pr("%s\n", __func__);
}
+const struct attribute_group *ev3_sensor_device_type_attr_groups[] = {
+ &legoev3_port_device_type_attr_grp,
+ NULL
+};
+
static struct tty_ldisc_ops legoev3_uart_ldisc = {
.magic = TTY_LDISC_MAGIC,
.name = "n_legoev3",
@@ -440,14 +687,24 @@ static struct tty_ldisc_ops legoev3_uart_ldisc = {
static int __init legoev3_uart_init(void)
{
- int err;
+ int err, i;
err = tty_register_ldisc(N_LEGOEV3, &legoev3_uart_ldisc);
if (err) {
pr_err("Could not register LEGOEV3 line discipline. (%d)\n",
err);
return err;
}
+ for (i = 0; i <= LEGOEV3_UART_TYPE_MAX; i++){
+ snprintf(legoev3_uart_sensor_type_names[i],
+ LEGOEV3_UART_DEVICE_TYPE_NAME_SIZE,
+ "ev3-uart-sensor-type-%d", i);
+ }
+ for (i = 0; i <= LEGOEV3_UART_TYPE_MAX; i++){
+ struct device_type *type = &legoev3_uart_sensor_device_types[i];
+ type->name = legoev3_uart_sensor_type_names[i];
+ type->groups = ev3_sensor_device_type_attr_groups;
+ }
pr_info("Registered LEGOEV3 line discipline. (%d)\n", N_LEGOEV3);
View
34 include/linux/legoev3/legoev3_uart.h
@@ -0,0 +1,34 @@
+/*
+ * tty line discipline for LEGO Mindstorms EV3 UART sensors
+ *
+ * Copyright (C) 2014 David Lechner <david@lechnology.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef _LINUX_LEGOEV3_UART_H_
+#define _LINUX_LEGOEV3_UART_H_
+
+/**
+ * struct legoev3_uart_sensor_platform_data
+ * @tty: The TTY device this sensor is connected to.
+ * @mode_info: Array of mode information for the sensor.
+ * @type: The type id of the sensor.
+ * @num_modes: The number of valid modes.
+ * @num_view_modes: The number of valid modes for data logging.
+ */
+struct legoev3_uart_sensor_platform_data {
+ struct tty_struct *tty;
+ struct legoev3_uart_mode_info *mode_info;
+ u8 num_modes;
+ u8 num_view_modes;
+};
+
+#endif /* _LINUX_LEGOEV3_UART_H_ */

0 comments on commit 0fe200c

Please sign in to comment.