static void brickpi_init_work(struct work_struct *work)
{
struct brickpi_data *data = container_of(work, struct brickpi_data,
poll_work);
int i, err;
for (i = 0; i < data->num_channels; i++) {
struct brickpi_channel_data *ch_data = &data->channel_data[i];
ch_data->data = data;
// TODO: add module parameter to get addresses
ch_data->address = i + 1;
ch_data->in_port[BRICKPI_PORT_1].ch_data = ch_data;
ch_data->in_port[BRICKPI_PORT_1].sensor_type =
BRICKPI_SENSOR_TYPE_FW_VERSION;
ch_data->in_port[BRICKPI_PORT_2].ch_data = ch_data;
ch_data->in_port[BRICKPI_PORT_2].sensor_type =
BRICKPI_SENSOR_TYPE_FW_VERSION;
err = brickpi_set_sensors(ch_data);
if (err < 0) {
dev_err(data->tty->dev,
"Failed to init while setting sensors. (%d)\n",
err);
return;
}
ch_data->out_port[BRICKPI_PORT_1].ch_data = ch_data;
ch_data->out_port[BRICKPI_PORT_2].ch_data = ch_data;
err = brickpi_get_values(ch_data);
if (err < 0) {
dev_err(data->tty->dev,
"Failed to init while getting values. (%d)\n",
err);
return;
}
ch_data->fw_version =
ch_data->in_port[BRICKPI_PORT_1].sensor_values[0];
debug_pr("address: %d, fw: %d\n", ch_data->address,
ch_data->fw_version);
err = brickpi_register_in_ports(ch_data, data->tty->dev);
if (err < 0) {
dev_err(data->tty->dev,
"Failed to register input ports (%d)",
err);
return;
}
err = brickpi_register_out_ports(ch_data, data->tty->dev);
if (err < 0) {
brickpi_unregister_in_ports(ch_data);
dev_err(data->tty->dev,
"Failed to register output ports (%d)",
err);
return;
}
ch_data->init_ok = true;
}
INIT_WORK(&data->poll_work, brickpi_poll_work);
hrtimer_start(&data->poll_timer, ktime_set(0, 0), HRTIMER_MODE_REL);
}
static void brickpi_poll_work(struct work_struct *work)
{
struct brickpi_data *data = container_of(work, struct brickpi_data,
poll_work);
int i, err;
if (data->closing)
return;
for (i = 0; i < data->num_channels; i++) {
struct brickpi_channel_data *ch_data = &data->channel_data[i];
if (ch_data->init_ok) {
err = brickpi_get_values(ch_data);
if (err < 0)
debug_pr("failed to get values for address %d. (%d)\n",
ch_data->address, err);
}
}
}
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);
}
static void ev3_uart_write_wakeup(struct tty_struct *tty)
{
debug_pr("%s\n", __func__);
}
int brickpi_get_values(struct brickpi_channel_data *ch_data)
{
struct brickpi_data *data = ch_data->data;
int i, j, err;
u8 port_size[NUM_BRICKPI_PORT];
if (data->closing)
return 0;
mutex_lock(&data->tx_mutex);
data->tx_buffer_tail = BRICKPI_TX_BUFFER_TAIL_INIT;
for (i = 0; i < NUM_BRICKPI_PORT; i++) {
brickpi_append_tx(data, 1, ch_data->out_port[i].motor_use_offset);
/* TODO: handle use_offset */
}
for (i = 0; i < NUM_BRICKPI_PORT; i++) {
brickpi_append_tx(data, 1, ch_data->out_port[i].motor_enabled);
brickpi_append_tx(data, 1, ch_data->out_port[i].motor_reversed);
brickpi_append_tx(data, 8, ch_data->out_port[i].motor_speed);
}
for (i = 0; i < NUM_BRICKPI_PORT; i++) {
struct brickpi_in_port_data *port = &ch_data->in_port[i];
int num_msg = port->num_i2c_msg;
if (port->sensor_type == BRICKPI_SENSOR_TYPE_NXT_I2C
|| port->sensor_type == BRICKPI_SENSOR_TYPE_NXT_I2C_9V)
{
for (j = 0; j < num_msg; j++) {
struct brickpi_i2c_msg_data *msg = &port->i2c_msg[j];
if (!(msg->settings & BRICKPI_I2C_SAME)) {
int k;
brickpi_append_tx(data, 4, msg->write_size);
brickpi_append_tx(data, 4, msg->read_size);
for (k = 0; k < msg->write_size; k++) {
brickpi_append_tx(data, 8, msg->write_data[k]);
}
}
}
}
}
err = brickpi_send_message(data, ch_data->address,
BRICK_PI_MESSAGE_GET_VALUES, 100);
if (err < 0) {
/* TODO: Set encoder offsets to 0 */
mutex_unlock(&data->tx_mutex);
return err;
}
data->rx_buffer_head = BRICKPI_RX_BUFFER_HEAD_INIT;
port_size[BRICKPI_PORT_1] = brickpi_read_rx(data, 5);
debug_pr("port_size[BRICKPI_PORT_1]: %u\n", port_size[BRICKPI_PORT_1]);
port_size[BRICKPI_PORT_2] = brickpi_read_rx(data, 5);
debug_pr("port_size[BRICKPI_PORT_2]: %u\n", port_size[BRICKPI_PORT_2]);
for (i = 0; i < NUM_BRICKPI_PORT; i++) {
u64 bits = brickpi_read_rx(data, port_size[i]);
s64 position = bits >> 1;
if (bits & 1)
position *= -1;
ch_data->out_port[i].motor_position = position;
debug_pr("motor_position[%d]: %d\n", i, (int)position);
if (ch_data->out_port[i].stop_at_target_position) {
if (((ch_data->out_port[i].motor_reversed)
&& position < (ch_data->out_port[i].target_position))
|| (!(ch_data->out_port[i].motor_reversed)
&& position > (ch_data->out_port[i].target_position)))
{
ch_data->out_port[i].motor_enabled = false;
}
}
}
for (i = 0; i < NUM_BRICKPI_PORT; i++) {
s32 *sensor_values = ch_data->in_port[i].sensor_values;
u8 *raw_data = ch_data->in_port[i].port.raw_data;
switch (ch_data->in_port[i].sensor_type) {
case BRICKPI_SENSOR_TYPE_NXT_TOUCH:
case BRICKPI_SENSOR_TYPE_NXT_TOUCH_DEBOUNCED:
sensor_values[0] = brickpi_read_rx(data, 1);
break;
case BRICKPI_SENSOR_TYPE_NXT_ULTRASONIC_CONT:
case BRICKPI_SENSOR_TYPE_NXT_ULTRASONIC_SS:
sensor_values[0] = brickpi_read_rx(data, 8);
break;
case BRICKPI_SENSOR_TYPE_NXT_COLOR_FULL:
sensor_values[0] = brickpi_read_rx(data, 3);
sensor_values[1] = brickpi_read_rx(data, 10);
sensor_values[2] = brickpi_read_rx(data, 10);
sensor_values[3] = brickpi_read_rx(data, 10);
sensor_values[4] = brickpi_read_rx(data, 10);
break;
case BRICKPI_SENSOR_TYPE_NXT_I2C:
case BRICKPI_SENSOR_TYPE_NXT_I2C_9V:
sensor_values[0] = brickpi_read_rx(data,
ch_data->in_port[i].num_i2c_msg);
for (j = 0; j < ch_data->in_port[i].num_i2c_msg; j++) {
if (sensor_values[0] & (1 << j)) {
int k;
struct brickpi_i2c_msg_data *msg =
&ch_data->in_port[i].i2c_msg[j];
for (k = 0; k < msg->read_size; k++) {
msg->read_data[k] =
brickpi_read_rx(data, 8);
}
}
}
break;
case BRICKPI_SENSOR_TYPE_EV3_US_M0:
case BRICKPI_SENSOR_TYPE_EV3_US_M1:
case BRICKPI_SENSOR_TYPE_EV3_US_M2:
case BRICKPI_SENSOR_TYPE_EV3_US_M3:
case BRICKPI_SENSOR_TYPE_EV3_US_M4:
case BRICKPI_SENSOR_TYPE_EV3_US_M5:
case BRICKPI_SENSOR_TYPE_EV3_US_M6:
case BRICKPI_SENSOR_TYPE_EV3_COLOR_M0:
case BRICKPI_SENSOR_TYPE_EV3_COLOR_M1:
case BRICKPI_SENSOR_TYPE_EV3_COLOR_M2:
case BRICKPI_SENSOR_TYPE_EV3_COLOR_M4:
case BRICKPI_SENSOR_TYPE_EV3_COLOR_M5:
case BRICKPI_SENSOR_TYPE_EV3_GYRO_M0:
case BRICKPI_SENSOR_TYPE_EV3_GYRO_M1:
case BRICKPI_SENSOR_TYPE_EV3_GYRO_M2:
case BRICKPI_SENSOR_TYPE_EV3_GYRO_M4:
case BRICKPI_SENSOR_TYPE_EV3_INFRARED_M0:
case BRICKPI_SENSOR_TYPE_EV3_INFRARED_M1:
case BRICKPI_SENSOR_TYPE_EV3_INFRARED_M3:
case BRICKPI_SENSOR_TYPE_EV3_INFRARED_M4:
case BRICKPI_SENSOR_TYPE_EV3_INFRARED_M5:
sensor_values[0] = brickpi_read_rx(data, 16);
/*
* There is a race condition in the BrickPi firmware v2 that
* causes these errors to be returned quite frequently. This
* is unfortunate because these may actually be legitimate
* values. Hopefully this can be fixed in future versions
* of the frimware, in which case we can add a version check
* here. But for now, we just ignore these values.
*/
if (sensor_values[0] == (s16)(-2))
continue;
if (sensor_values[0] == (s16)(-4))
continue;
break;
case BRICKPI_SENSOR_TYPE_EV3_COLOR_M3:
case BRICKPI_SENSOR_TYPE_EV3_GYRO_M3:
case BRICKPI_SENSOR_TYPE_EV3_INFRARED_M2:
sensor_values[0] = brickpi_read_rx(data, 32);
/* see comment above */
if (sensor_values[0] == (s32)(-2))
continue;
if (sensor_values[0] == (s32)(-4))
continue;
break;
case BRICKPI_SENSOR_TYPE_EV3_TOUCH:
case BRICKPI_SENSOR_TYPE_EV3_TOUCH_DEBOUNCED:
/*
* The EV3 Touch sensor returns a value of 0x07 or 0x10
* when pressed, so this converts it to an approx. mV
* value that is close enough for the ev3-analog-sensor
* driver to interpret correctly.
*/
sensor_values[0] = brickpi_read_rx(data, 16) << 8;
break;
default:
/* NXT Analog expects value in mV */
if (ch_data->in_port[i].sensor_type <= BRICKPI_SENSOR_TYPE_NXT_ANALOG_MAX)
sensor_values[0] = brickpi_read_rx(data, 10) * 5000 / 1024;
else
sensor_values[0] = brickpi_read_rx(data, 10);
break;
}
debug_pr("ch_data->sensor_values[%d][0]: %ld\n", i,
sensor_values[0]);
if (!raw_data)
continue;
if (ch_data->in_port[i].sensor_type == BRICKPI_SENSOR_TYPE_NXT_I2C
|| ch_data->in_port[i].sensor_type == BRICKPI_SENSOR_TYPE_NXT_I2C_9V)
{
memcpy(raw_data, ch_data->in_port[i].i2c_msg[0].read_data,
ch_data->in_port[i].i2c_msg[0].read_size);
} else {
memcpy(raw_data, sensor_values,
sizeof(s32) * NUM_BRICKPI_SENSOR_VALUES);
}
lego_port_call_raw_data_func(&ch_data->in_port[i].port);
}
mutex_unlock(&data->tx_mutex);
return 0;
}
static void legoev3_uart_receive_buf(struct tty_struct *tty,
const unsigned char *cp, char *fp,
int count)
{
struct legoev3_uart_port_data *port = tty->disc_data;
int i = 0;
int j, speed;
u8 cmd, cmd2, type, mode, msg_type, msg_size, chksum;
#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
* for a valid TYPE command.
*/
while (!port->synced) {
if (i + 2 >= count)
return;
cmd = cp[i++];
if (cmd != (LEGOEV3_UART_MSG_TYPE_CMD | LEGOEV3_UART_CMD_TYPE))
continue;
type = cp[i];
if (!type || type > LEGOEV3_UART_TYPE_MAX)
continue;
chksum = 0xFF ^ cmd ^ type;
if (cp[i + 1] != chksum)
continue;
port->ms.num_modes = 1;
port->ms.num_view_modes = 1;
for (j = 0; j <= MSENSOR_MODE_MAX; j++)
port->mode_info[j] = legoev3_uart_default_mode_info;
port->ms.type_id = type;
port->info_flags = LEGOEV3_UART_INFO_FLAG_CMD_TYPE;
port->synced = 1;
port->info_done = 0;
port->write_ptr = 0;
port->data_rec = 0;
port->num_data_err = 0;
i += 2;
}
if (!port->synced)
return;
/*
* Once we are synced, we keep reading data until we have read
* a complete command.
*/
while (i < count) {
if (port->write_ptr >= LEGOEV3_UART_BUFFER_SIZE) {
port->last_err = "Receive buffer overrun.";
goto err_invalid_state;
}
port->buffer[port->write_ptr++] = cp[i++];
}
/*
* Process all complete messages that have been received.
*/
while ((msg_size = legoev3_uart_msg_size(port->buffer[0]))
<= port->write_ptr)
{
#ifdef DEBUG
printk("processing: ");
for (i = 0; i < port->write_ptr; i++)
printk("0x%02x ", port->buffer[i]);
printk(" (%d)\n", port->write_ptr);
printk("msg_size:%d\n", msg_size);
#endif
/*
* The IR sensor sends 0xFF after SYNC (0x00). If these two
* bytes get split between two interrupts, then it will throw
* us off and prevent the sensor from being recognized. So,
* if the first byte is 0xFF, we just ignore it and continue
* with our loop.
*/
if (port->buffer[0] == 0xFF) {
msg_size = 1;
goto err_split_sync_checksum;
}
if (msg_size > MSENSOR_RAW_DATA_SIZE + 2) {
port->last_err = "Bad message size.";
goto err_invalid_state;
}
msg_type = port->buffer[0] & LEGOEV3_UART_MSG_TYPE_MASK;
cmd = port->buffer[0] & LEGOEV3_UART_MSG_CMD_MASK;
mode = cmd;
cmd2 = port->buffer[1];
if (msg_size > 1) {
chksum = 0xFF;
for (i = 0; i < msg_size - 1; i++)
chksum ^= port->buffer[i];
debug_pr("chksum:%d, actual:%d\n",
chksum, port->buffer[msg_size - 1]);
/*
* The LEGO EV3 color sensor (type 29) sends bad checksums
* for RGB-RAW data (mode 4). The check here could be
* improved if someone can find a pattern.
*/
if (chksum != port->buffer[msg_size - 1]
&& port->ms.type_id != 29 && port->buffer[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 LEGOEV3_UART_MSG_TYPE_SYS:
debug_pr("SYS:%d\n", port->buffer[0] & LEGOEV3_UART_MSG_CMD_MASK);
switch(cmd) {
case LEGOEV3_UART_SYS_SYNC:
/* IR sensor (type 33) sends checksum after SYNC */
if (msg_size > 1 && (cmd ^ cmd2) == 0xFF)
msg_size++;
break;
case LEGOEV3_UART_SYS_ACK:
if (!port->ms.num_modes) {
port->last_err = "Received ACK before all mode INFO.";
goto err_invalid_state;
}
if ((port->info_flags & LEGOEV3_UART_INFO_FLAG_REQUIRED)
!= LEGOEV3_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(LEGOEV3_UART_SEND_ACK_DELAY));
port->info_done = 1;
break;
}
break;
case LEGOEV3_UART_MSG_TYPE_CMD:
debug_pr("CMD:%d\n", cmd);
switch (cmd) {
case LEGOEV3_UART_CMD_MODES:
if (test_and_set_bit(LEGOEV3_UART_INFO_BIT_CMD_MODES,
&port->info_flags))
{
port->last_err = "Received duplicate modes INFO.";
goto err_invalid_state;
}
if (!cmd2 || cmd2 > MSENSOR_MODE_MAX) {
port->last_err = "Number of modes is out of range.";
goto err_invalid_state;
}
port->ms.num_modes = cmd2 + 1;
if (msg_size > 3)
port->ms.num_view_modes = port->buffer[2] + 1;
else
port->ms.num_view_modes = port->ms.num_modes;
debug_pr("num_modes:%d, num_view_modes:%d\n",
port->ms.num_modes, port->ms.num_view_modes);
break;
case LEGOEV3_UART_CMD_SPEED:
if (test_and_set_bit(LEGOEV3_UART_INFO_BIT_CMD_SPEED,
&port->info_flags))
{
port->last_err = "Received duplicate speed INFO.";
goto err_invalid_state;
}
speed = *(int*)(port->buffer + 1);
if (speed < LEGOEV3_UART_SPEED_MIN
|| speed > LEGOEV3_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 LEGOEV3_UART_MSG_TYPE_INFO:
debug_pr("INFO:%d, mode:%d\n", cmd2, mode);
switch (cmd2) {
case LEGOEV3_UART_INFO_NAME:
port->info_flags &= ~LEGOEV3_UART_INFO_FLAG_ALL_INFO;
if (port->buffer[2] < 'A' || port->buffer[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.
*/
port->buffer[msg_size - 1] = 0;
if (strlen(port->buffer + 2) > LEGOEV3_UART_MODE_NAME_SIZE) {
port->last_err = "Name is too long.";
goto err_invalid_state;
}
snprintf(port->mode_info[mode].name,
LEGOEV3_UART_MODE_NAME_SIZE + 1, "%s",
port->buffer + 2);
port->mode = mode;
port->info_flags |= LEGOEV3_UART_INFO_FLAG_INFO_NAME;
debug_pr("mode %d name:%s\n",
mode, port->mode_info[mode].port_name);
break;
case LEGOEV3_UART_INFO_RAW:
if (port->mode != mode) {
port->last_err = "Received INFO for incorrect mode.";
goto err_invalid_state;
}
if (test_and_set_bit(LEGOEV3_UART_INFO_BIT_INFO_RAW,
&port->info_flags))
{
port->last_err = "Received duplicate raw scaling INFO.";
goto err_invalid_state;
}
port->raw_min = *(u32 *)(port->buffer + 2);
port->raw_max = *(u32 *)(port->buffer + 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 LEGOEV3_UART_INFO_PCT:
if (port->mode != mode) {
port->last_err = "Received INFO for incorrect mode.";
goto err_invalid_state;
}
if (test_and_set_bit(LEGOEV3_UART_INFO_BIT_INFO_PCT,
&port->info_flags))
{
port->last_err = "Received duplicate percent scaling INFO.";
goto err_invalid_state;
}
port->pct_min = *(u32 *)(port->buffer + 2);
port->pct_max = *(u32 *)(port->buffer + 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 LEGOEV3_UART_INFO_SI:
if (port->mode != mode) {
port->last_err = "Received INFO for incorrect mode.";
goto err_invalid_state;
}
if (test_and_set_bit(LEGOEV3_UART_INFO_BIT_INFO_SI,
&port->info_flags))
{
port->last_err = "Received duplicate SI scaling INFO.";
goto err_invalid_state;
}
port->si_min = *(u32 *)(port->buffer + 2);
port->si_max = *(u32 *)(port->buffer + 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 LEGOEV3_UART_INFO_UNITS:
if (port->mode != mode) {
port->last_err = "Received INFO for incorrect mode.";
goto err_invalid_state;
}
if (test_and_set_bit(LEGOEV3_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.
*/
port->buffer[msg_size - 1] = 0;
snprintf(port->mode_info[mode].units,
MSENSOR_UNITS_SIZE + 1, "%s",
port->buffer + 2);
debug_pr("mode %d units:%s\n",
mode, port->mode_info[mode].units);
break;
case LEGOEV3_UART_INFO_FORMAT:
if (port->mode != mode) {
port->last_err = "Received INFO for incorrect mode.";
goto err_invalid_state;
}
if (test_and_set_bit(LEGOEV3_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 = port->buffer[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 & LEGOEV3_UART_INFO_FLAG_REQUIRED)
!= LEGOEV3_UART_INFO_FLAG_REQUIRED) {
port->last_err = "Did not receive all required INFO.";
goto err_invalid_state;
}
switch (port->buffer[3]) {
case LEGOEV3_UART_DATA_8:
port->mode_info[mode].data_type =
(port->mode_info[mode].si_min < 0) ?
MSENSOR_DATA_S8 : MSENSOR_DATA_U8;
break;
case LEGOEV3_UART_DATA_16:
port->mode_info[mode].data_type =
(port->mode_info[mode].si_min < 0) ?
MSENSOR_DATA_S16 : MSENSOR_DATA_U16;
break;
case LEGOEV3_UART_DATA_32:
port->mode_info[mode].data_type =
(port->mode_info[mode].si_min < 0) ?
MSENSOR_DATA_S32 : MSENSOR_DATA_U32;
break;
case LEGOEV3_UART_DATA_FLOAT:
port->mode_info[mode].data_type =
MSENSOR_DATA_FLOAT;
break;
default:
port->last_err = "Invalid data type.";
goto err_invalid_state;
}
port->mode_info[mode].figures = port->buffer[4];
port->mode_info[mode].decimals = port->buffer[5];
if (port->info_flags & LEGOEV3_UART_INFO_FLAG_INFO_RAW) {
port->mode_info[mode].raw_min =
msensor_ftoi(port->raw_min,
port->mode_info[mode].decimals);
port->mode_info[mode].raw_max =
msensor_ftoi(port->raw_max,
port->mode_info[mode].decimals);
}
if (port->info_flags & LEGOEV3_UART_INFO_FLAG_INFO_PCT) {
port->mode_info[mode].pct_min =
msensor_ftoi(port->pct_min,
port->mode_info[mode].decimals);
port->mode_info[mode].pct_max =
msensor_ftoi(port->pct_max,
port->mode_info[mode].decimals);
}
if (port->info_flags & LEGOEV3_UART_INFO_FLAG_INFO_SI) {
port->mode_info[mode].si_min =
msensor_ftoi(port->si_min,
port->mode_info[mode].decimals);
port->mode_info[mode].si_max =
msensor_ftoi(port->si_max,
port->mode_info[mode].decimals);
}
if (port->mode)
port->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);
break;
}
break;
case LEGOEV3_UART_MSG_TYPE_DATA:
debug_pr("DATA:%d\n", port->buffer[0] & LEGOEV3_UART_MSG_CMD_MASK);
if (!port->info_done) {
port->last_err = "Received DATA before INFO was complete.";
goto err_invalid_state;
}
if (mode > MSENSOR_MODE_MAX) {
port->last_err = "Invalid mode received.";
goto err_invalid_state;
}
port->mode = mode;
memcpy(port->mode_info[mode].raw_data,
port->buffer + 1, msg_size - 2);
port->data_rec = 1;
if (port->num_data_err)
port->num_data_err--;
break;
}
err_bad_data_msg_checksum:
if (port->info_done && port->num_data_err > LEGOEV3_UART_MAX_DATA_ERR)
goto err_invalid_state;
err_split_sync_checksum:
/*
* If there is leftover data, we move it to the beginning
* of the buffer.
*/
for (i = 0; i + msg_size < port->write_ptr; i++)
port->buffer[i] = port->buffer[i + msg_size];
port->write_ptr = i;
}
return;
err_invalid_state:
port->synced = 0;
port->new_baud_rate = LEGOEV3_UART_SPEED_MIN;
schedule_delayed_work(&port->change_bitrate_work,
msecs_to_jiffies(LEGOEV3_UART_SET_BITRATE_DELAY));
}
