static ssize_t ev3_uart_direct_write(void *context, char *data, loff_t off,
size_t count)
{
struct tty_struct *tty = context;
char uart_data[EV3_UART_MAX_MESSAGE_SIZE];
int size, i, err;
if (off != 0 || count > EV3_UART_MAX_DATA_SIZE)
return -EINVAL;
if (count == 0)
return count;
memset(uart_data + 1, 0, EV3_UART_MAX_DATA_SIZE);
memcpy(uart_data + 1, data, count);
if (count <= 2)
size = count;
else if (count <= 4)
size = 4;
else if (count <= 8)
size = 8;
else if (count <= 16)
size = 16;
else
size = 32;
uart_data[0] = ev3_uart_set_msg_hdr(EV3_UART_MSG_TYPE_CMD, size,
EV3_UART_CMD_WRITE);
data[size + 1] = 0xFF;
for (i = 0; i <= size; i++)
uart_data[size + 1] ^= uart_data[i];
set_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
err = tty->ops->write(tty, uart_data, size + 2);
if (err < 0)
return err;
return count;
}
int ev3_uart_set_mode(void *context, const u8 mode)
{
struct tty_struct *tty = context;
struct ev3_uart_port_data *port;
const int data_size = 3;
u8 data[data_size];
int retries = 10;
int ret;
if (!tty)
return -ENODEV;
port = tty->disc_data;
if (!port->synced || !port->info_done)
return -ENODEV;
if (mode >= port->sensor.num_modes)
return -EINVAL;
if (!completion_done(&port->set_mode_completion))
return -EBUSY;
data[0] = ev3_uart_set_msg_hdr(EV3_UART_MSG_TYPE_CMD,
data_size - 2,
EV3_UART_CMD_SELECT);
data[1] = mode;
data[2] = 0xFF ^ data[0] ^ data[1];
port->new_mode = mode;
reinit_completion(&port->set_mode_completion);
while (retries--) {
set_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
ret = tty->ops->write(tty, data, data_size);
if (ret < 0)
return ret;
ret = wait_for_completion_timeout(&port->set_mode_completion,
msecs_to_jiffies(50));
if (ret)
break;
}
port->set_mode_completion.done++;
if (!ret)
return -ETIMEDOUT;
port->requested_mode = mode;
return 0;
}