static void ev3_uart_handle_rx_data(struct work_struct *work)
{
struct ev3_uart_port_data *port =
container_of(work, struct ev3_uart_port_data, rx_data_work);
struct circ_buf *cb = &port->circ_buf;
u8 message[EV3_UART_MAX_MESSAGE_SIZE + 2];
int count = CIRC_CNT(cb->head, cb->tail, EV3_UART_BUFFER_SIZE);
int i, speed, size_to_end;
u8 cmd, cmd2, type, mode, msg_type, msg_size, chksum;
#ifdef DEBUG
printk("received: ");
for (i = 0; i < count; i++) {
cmd = cb->buf[(cb->tail + i) % EV3_UART_BUFFER_SIZE];
if (cmd >= 32 && cmd < 127)
printk("%c ", cmd);
else
printk("0x%02x ", cmd);
}
printk("(%d)\n", count);
#endif
/*
* To get in sync with the data stream from the sensor, we look
* for a valid TYPE command.
*/
while (!port->synced) {
if (count < 3)
return;
cmd = cb->buf[cb->tail];
cb->tail++;
if (cb->tail >= EV3_UART_BUFFER_SIZE)
cb->tail = 0;
count--;
if (cmd != (EV3_UART_MSG_TYPE_CMD | EV3_UART_CMD_TYPE))
continue;
type = cb->buf[cb->tail];
if (!type || type > EV3_UART_TYPE_MAX)
continue;
chksum = 0xFF ^ cmd ^ type;
if ((u8)cb->buf[(cb->tail + 1) % EV3_UART_BUFFER_SIZE] != chksum)
continue;
port->sensor.num_modes = 1;
port->sensor.num_view_modes = 1;
for (i = 0; i <= EV3_UART_MODE_MAX; i++)
port->mode_info[i] = ev3_uart_default_mode_info;
port->type_id = type;
/* look up well-known driver names */
port->device_name[0] = 0;
for (i = 0; i < NUM_LEGO_EV3_SENSOR_TYPES; i++) {
if (type == ev3_uart_sensor_defs[i].type_id) {
snprintf(port->device_name, LEGO_SENSOR_NAME_SIZE,
"%s", ev3_uart_sensor_defs[i].name);
break;
}
}
/* or use generic name if well-known name is not found */
if (!port->device_name[0])
snprintf(port->device_name, LEGO_SENSOR_NAME_SIZE,
EV3_UART_SENSOR_NAME("%u"), type);
port->info_flags = EV3_UART_INFO_FLAG_CMD_TYPE;
port->synced = 1;
port->info_done = 0;
port->data_rec = 0;
port->num_data_err = 0;
cb->tail = (cb->tail + 2) % EV3_UART_BUFFER_SIZE;
count -= 2;
}
if (!port->synced)
return;
while (count > 0)
{
/*
* Sometimes we get 0xFF after switching baud rates, so just
* ignore it.
*/
if ((u8)cb->buf[cb->tail] == 0xFF) {
cb->tail++;
if (cb->tail >= EV3_UART_BUFFER_SIZE)
cb->tail = 0;
count--;
continue;
}
msg_size = ev3_uart_msg_size((u8)cb->buf[cb->tail]);
if (msg_size > count)
break;
size_to_end = CIRC_CNT_TO_END(cb->head, cb->tail, EV3_UART_BUFFER_SIZE);
if (msg_size > size_to_end) {
memcpy(message, cb->buf + cb->tail, size_to_end);
memcpy(message + size_to_end, cb->buf, msg_size - size_to_end);
cb->tail = msg_size - size_to_end;
} else {
memcpy(message, cb->buf + cb->tail, msg_size);
cb->tail += msg_size;
if (cb->tail >= EV3_UART_BUFFER_SIZE)
cb->tail = 0;
}
count -= msg_size;
#ifdef DEBUG
printk("processing: ");
for (i = 0; i < msg_size; i++)
printk("0x%02x ", message[i]);
printk(" (%d)\n", msg_size);
#endif
if (msg_size > EV3_UART_MAX_MESSAGE_SIZE) {
port->last_err = "Bad message size.";
goto err_invalid_state;
}
msg_type = message[0] & EV3_UART_MSG_TYPE_MASK;
cmd = message[0] & EV3_UART_MSG_CMD_MASK;
mode = cmd;
cmd2 = message[1];
if (msg_size > 1) {
chksum = 0xFF;
for (i = 0; i < msg_size - 1; i++)
chksum ^= message[i];
debug_pr("chksum:%d, actual:%d\n",
chksum, message[msg_size - 1]);
/*
* The LEGO EV3 color sensor sends bad checksums
* for RGB-RAW data (mode 4). The check here could be
* improved if someone can find a pattern.
*/
if (chksum != message[msg_size - 1]
&& port->type_id != EV3_UART_TYPE_ID_COLOR
&& message[0] != 0xDC)
{
port->last_err = "Bad checksum.";
if (port->info_done) {
port->num_data_err++;
goto err_bad_data_msg_checksum;
} else
goto err_invalid_state;
}
}
switch (msg_type) {
case EV3_UART_MSG_TYPE_SYS:
debug_pr("SYS:%d\n", message[0] & EV3_UART_MSG_CMD_MASK);
switch(cmd) {
case EV3_UART_SYS_SYNC:
/* IR sensor (type 33) sends checksum after SYNC */
if (msg_size > 1 && (cmd ^ cmd2) == 0xFF)
msg_size++;
break;
case EV3_UART_SYS_ACK:
if (!port->sensor.num_modes) {
port->last_err = "Received ACK before all mode INFO.";
goto err_invalid_state;
}
if ((port->info_flags & EV3_UART_INFO_FLAG_REQUIRED)
!= EV3_UART_INFO_FLAG_REQUIRED)
{
port->last_err = "Did not receive all required INFO.";
goto err_invalid_state;
}
schedule_delayed_work(&port->send_ack_work,
msecs_to_jiffies(EV3_UART_SEND_ACK_DELAY));
port->info_done = 1;
return;
}
break;
case EV3_UART_MSG_TYPE_CMD:
debug_pr("CMD:%d\n", cmd);
switch (cmd) {
case EV3_UART_CMD_MODES:
if (test_and_set_bit(EV3_UART_INFO_BIT_CMD_MODES,
&port->info_flags))
{
port->last_err = "Received duplicate modes INFO.";
goto err_invalid_state;
}
if (!cmd2 || cmd2 > EV3_UART_MODE_MAX) {
port->last_err = "Number of modes is out of range.";
goto err_invalid_state;
}
port->sensor.num_modes = cmd2 + 1;
if (msg_size > 3)
port->sensor.num_view_modes = message[2] + 1;
else
port->sensor.num_view_modes = port->sensor.num_modes;
debug_pr("num_modes:%d, num_view_modes:%d\n",
port->sensor.num_modes, port->sensor.num_view_modes);
break;
case EV3_UART_CMD_SPEED:
if (test_and_set_bit(EV3_UART_INFO_BIT_CMD_SPEED,
&port->info_flags))
{
port->last_err = "Received duplicate speed INFO.";
goto err_invalid_state;
}
speed = *(int*)(message + 1);
if (speed < EV3_UART_SPEED_MIN
|| speed > EV3_UART_SPEED_MAX)
{
port->last_err = "Speed is out of range.";
goto err_invalid_state;
}
port->new_baud_rate = speed;
debug_pr("speed:%d\n", speed);
break;
default:
port->last_err = "Unknown command.";
goto err_invalid_state;
}
break;
case EV3_UART_MSG_TYPE_INFO:
debug_pr("INFO:%d, mode:%d\n", cmd2, mode);
switch (cmd2) {
case EV3_UART_INFO_NAME:
port->info_flags &= ~EV3_UART_INFO_FLAG_ALL_INFO;
if (message[2] < 'A' || message[2] > 'z') {
port->last_err = "Invalid name INFO.";
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.
*/
message[msg_size - 1] = 0;
if (strlen(message + 2) > EV3_UART_MODE_NAME_SIZE) {
port->last_err = "Name is too long.";
goto err_invalid_state;
}
snprintf(port->mode_info[mode].name,
EV3_UART_MODE_NAME_SIZE + 1, "%s",
message + 2);
if (port->sensor.mode != mode) {
port->sensor.mode = mode;
kobject_uevent(&port->sensor.dev.kobj,
KOBJ_CHANGE);
}
port->info_flags |= EV3_UART_INFO_FLAG_INFO_NAME;
debug_pr("mode %d name:%s\n",
mode, port->sensor.address);
break;
case EV3_UART_INFO_RAW:
if (port->sensor.mode != mode) {
port->last_err = "Received INFO for incorrect mode.";
goto err_invalid_state;
}
if (test_and_set_bit(EV3_UART_INFO_BIT_INFO_RAW,
&port->info_flags))
{
port->last_err = "Received duplicate raw scaling INFO.";
goto err_invalid_state;
}
port->raw_min = *(u32 *)(message + 2);
port->raw_max = *(u32 *)(message + 6);
debug_pr("mode %d raw_min:%08x, raw_max:%08x\n",
mode, port->mode_info[mode].raw_min,
port->mode_info[mode].raw_max);
break;
case EV3_UART_INFO_PCT:
if (port->sensor.mode != mode) {
port->last_err = "Received INFO for incorrect mode.";
goto err_invalid_state;
}
if (test_and_set_bit(EV3_UART_INFO_BIT_INFO_PCT,
&port->info_flags))
{
port->last_err = "Received duplicate percent scaling INFO.";
goto err_invalid_state;
}
port->pct_min = *(u32 *)(message + 2);
port->pct_max = *(u32 *)(message + 6);
debug_pr("mode %d pct_min:%08x, pct_max:%08x\n",
mode, port->mode_info[mode].pct_min,
port->mode_info[mode].pct_max);
break;
case EV3_UART_INFO_SI:
if (port->sensor.mode != mode) {
port->last_err = "Received INFO for incorrect mode.";
goto err_invalid_state;
}
if (test_and_set_bit(EV3_UART_INFO_BIT_INFO_SI,
&port->info_flags))
{
port->last_err = "Received duplicate SI scaling INFO.";
goto err_invalid_state;
}
port->si_min = *(u32 *)(message + 2);
port->si_max = *(u32 *)(message + 6);
debug_pr("mode %d si_min:%08x, si_max:%08x\n",
mode, port->mode_info[mode].si_min,
port->mode_info[mode].si_max);
break;
case EV3_UART_INFO_UNITS:
if (port->sensor.mode != mode) {
port->last_err = "Received INFO for incorrect mode.";
goto err_invalid_state;
}
if (test_and_set_bit(EV3_UART_INFO_BIT_INFO_UNITS,
&port->info_flags))
{
port->last_err = "Received duplicate SI units INFO.";
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.
*/
message[msg_size - 1] = 0;
snprintf(port->mode_info[mode].units,
EV3_UART_UNITS_SIZE + 1, "%s",
message + 2);
debug_pr("mode %d units:%s\n",
mode, port->mode_info[mode].units);
break;
case EV3_UART_INFO_FORMAT:
if (port->sensor.mode != mode) {
port->last_err = "Received INFO for incorrect mode.";
goto err_invalid_state;
}
if (test_and_set_bit(EV3_UART_INFO_BIT_INFO_FORMAT,
&port->info_flags))
{
port->last_err = "Received duplicate format INFO.";
goto err_invalid_state;
}
port->mode_info[mode].data_sets = message[2];
if (!port->mode_info[mode].data_sets) {
port->last_err = "Invalid number of data sets.";
goto err_invalid_state;
}
if (msg_size < 7) {
port->last_err = "Invalid format message size.";
goto err_invalid_state;
}
if ((port->info_flags & EV3_UART_INFO_FLAG_REQUIRED)
!= EV3_UART_INFO_FLAG_REQUIRED) {
port->last_err = "Did not receive all required INFO.";
goto err_invalid_state;
}
switch (message[3]) {
case EV3_UART_DATA_8:
port->mode_info[mode].data_type = LEGO_SENSOR_DATA_S8;
break;
case EV3_UART_DATA_16:
port->mode_info[mode].data_type = LEGO_SENSOR_DATA_S16;
break;
case EV3_UART_DATA_32:
port->mode_info[mode].data_type = LEGO_SENSOR_DATA_S32;
break;
case EV3_UART_DATA_FLOAT:
port->mode_info[mode].data_type = LEGO_SENSOR_DATA_FLOAT;
break;
default:
port->last_err = "Invalid data type.";
goto err_invalid_state;
}
port->mode_info[mode].figures = message[4];
port->mode_info[mode].decimals = message[5];
if (port->info_flags & EV3_UART_INFO_FLAG_INFO_RAW) {
port->mode_info[mode].raw_min =
lego_sensor_ftoi(port->raw_min, 0);
port->mode_info[mode].raw_max =
lego_sensor_ftoi(port->raw_max, 0);
}
if (port->info_flags & EV3_UART_INFO_FLAG_INFO_PCT) {
port->mode_info[mode].pct_min =
lego_sensor_ftoi(port->pct_min, 0);
port->mode_info[mode].pct_max =
lego_sensor_ftoi(port->pct_max, 0);
}
if (port->info_flags & EV3_UART_INFO_FLAG_INFO_SI) {
port->mode_info[mode].si_min =
lego_sensor_ftoi(port->si_min,
port->mode_info[mode].decimals);
port->mode_info[mode].si_max =
lego_sensor_ftoi(port->si_max,
port->mode_info[mode].decimals);
}
if (port->sensor.mode)
port->sensor.mode--;
debug_pr("mode %d - data_sets:%d, data_type:%d, figures:%d, decimals:%d\n",
mode, port->mode_info[mode].data_sets,
port->mode_info[mode].data_type,
port->mode_info[mode].figures,
port->mode_info[mode].decimals);
debug_pr("raw_min: %d, raw_max: %d\n",
port->mode_info[mode].raw_min,
port->mode_info[mode].raw_max);
debug_pr("pct_min: %d, pct_max: %d\n",
port->mode_info[mode].pct_min,
port->mode_info[mode].pct_max);
debug_pr("si_min: %d, si_max: %d\n",
port->mode_info[mode].si_min,
port->mode_info[mode].si_max);
break;
}
break;
case EV3_UART_MSG_TYPE_DATA:
debug_pr("DATA:%d\n", message[0] & EV3_UART_MSG_CMD_MASK);
if (!port->info_done) {
port->last_err = "Received DATA before INFO was complete.";
goto err_invalid_state;
}
if (mode > EV3_UART_MODE_MAX) {
port->last_err = "Invalid mode received.";
goto err_invalid_state;
}
if (mode != port->sensor.mode) {
if (mode == port->new_mode) {
port->sensor.mode = mode;
kobject_uevent(&port->sensor.dev.kobj,
KOBJ_CHANGE);
} else {
port->last_err = "Unexpected mode.";
goto err_invalid_state;
}
}
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);
port->data_rec = 1;
if (port->num_data_err)
port->num_data_err--;
break;
}
err_bad_data_msg_checksum:
count = CIRC_CNT(cb->head, cb->tail, EV3_UART_BUFFER_SIZE);
}
return;
err_invalid_state:
port->synced = 0;
port->new_baud_rate = EV3_UART_SPEED_MIN;
schedule_work(&port->change_bitrate_work);
}
return err;
}
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);
unregister_lego_sensor(&data->sensor);
dev_set_drvdata(&ldev->dev, NULL);
kfree(data);
return 0;
}
static const struct lego_device_id ev3_uart_sensor_device_ids[] = {
{ EV3_UART_SENSOR_NAME("29"), LEGO_EV3_COLOR },
{ EV3_UART_SENSOR_NAME("30"), LEGO_EV3_ULTRASONIC },
{ EV3_UART_SENSOR_NAME("32"), LEGO_EV3_GYRO },
{ EV3_UART_SENSOR_NAME("33"), LEGO_EV3_INFRARED },
};
struct lego_device_driver ev3_uart_sensor_driver = {
.probe = ev3_uart_sensor_probe,
.remove = ev3_uart_sensor_remove,
.driver = {
.name = "ev3-uart-sensor",
.owner = THIS_MODULE,
},
.id_table = ev3_uart_sensor_device_ids,
};
lego_device_driver(ev3_uart_sensor_driver);
