diff --git a/drivers/misc/rm-pogo/pogo.h b/drivers/misc/rm-pogo/pogo.h index bd85f8dfa28b..c11b602ec068 100644 --- a/drivers/misc/rm-pogo/pogo.h +++ b/drivers/misc/rm-pogo/pogo.h @@ -34,6 +34,12 @@ #define ONE_WIRE_MCU_MSG_SIZE 280 #define MAX_POGO_DEV_NAME 16 +/* Maximum number of maxium-size incoming messages that can be buffered while + * waiting to be handled by FSM. Actual number will be higher since most + * messages are a lot smaller than ONE_WIRE_MCU_MSG_SIZE. + */ +#define NUM_MSGS_RECV_BUFFER 4 + #define ONE_WIRE_UART_ACK 1 #define ONE_WIRE_UART_LISTEN 2 #define POGO_TX_RETRY_LIMIT 3 @@ -326,12 +332,12 @@ int __must_check pogo_onewire_write(struct rm_pogo_data *data, u8 cmd, u8 ext, const unsigned char *msg, size_t count, bool ack_required); -static inline u32 packet_payload_len(u8 *msg) +static inline u32 packet_payload_len(const u8 *msg) { return msg[0] | ((msg[1] & 0xf) << 8); } -static inline u32 packet_len(u8 *msg) +static inline u32 packet_len(const u8 *msg) { /* 3 is for two length bytes and command*/ return packet_payload_len(msg) + 3; diff --git a/drivers/misc/rm-pogo/pogo_fsm.c b/drivers/misc/rm-pogo/pogo_fsm.c index bccde0058fca..0636d52f874b 100644 --- a/drivers/misc/rm-pogo/pogo_fsm.c +++ b/drivers/misc/rm-pogo/pogo_fsm.c @@ -60,24 +60,49 @@ const attribute_storage_id_t dev_info_attrs[] = { ATTRIBUTE_ID_DEVICE_ID, ATTRIBUTE_ID_RM_SERIAL_NUMBER, }; -#define GET_AND_VERIFY_MSG(MSG, MSG_SIZE, RETURN, FSM_ERR) \ -do { \ - count = kfifo_out(&pdata->read_fifo, MSG, MSG_SIZE); \ - if (count < 2) { \ - dev_err(pdata->dev, "Ack message length < 2!\n"); \ - if (FSM_ERR) \ - pdata->fsm_err = true; \ - return RETURN; \ - } \ - len = packet_payload_len(MSG); \ - if (count < len + 3) {\ - dev_err(pdata->dev, "Ack message length:%d, expected:%d!\n", \ - count, len + 3); \ - if (FSM_ERR) \ - pdata->fsm_err = true; \ - return RETURN; \ - } \ -} while(0) +/* + * If the payload size doesn't correspond to the size specified in the header + * we can't be sure of the rest of the buffer, so abort. entry_idle will + * reset read_fifo. + * + * This macro fetches at most once message from the buffer. Calls return with + * RETURN if buffer is empty or if the data found in the buffer does not match + * expectations. + */ +#define GET_AND_VERIFY_MSG(MSG, MSG_SIZE, RETURN, FSM_ERR) \ + do { \ + memset(msg, 0, MSG_SIZE); \ + count = kfifo_out(&pdata->read_fifo, MSG, \ + 3 /* header size */); \ + if (count != 3) { \ + dev_dbg(pdata->dev, \ + "%s: Header length != 3. Got %d\n", __func__, \ + count); \ + if (FSM_ERR) \ + pdata->fsm_err = true; \ + return RETURN; \ + } \ + \ + len = packet_payload_len(MSG); \ + if (len > MSG_SIZE - 3) { \ + dev_err(pdata->dev, \ + "%s: Message length: %d > %d, too large\n", \ + __func__, len, MSG_SIZE - 3); \ + if (FSM_ERR) \ + pdata->fsm_err = true; \ + return RETURN; \ + } \ + \ + count += kfifo_out(&pdata->read_fifo, MSG + 3, len); \ + if (count != 3 + len) { \ + dev_err(pdata->dev, \ + "%s: Message payload length:%d, expected:%d!\n", \ + __func__, count, len); \ + if (FSM_ERR) \ + pdata->fsm_err = true; \ + return RETURN; \ + } \ + } while (0) /****************************************************************************** * EVENT FUNCTIONS @@ -719,30 +744,31 @@ static bool fsm_exit_uart_keyboard(struct rm_pogo_data *pdata) return true; } -static void fsm_routine_uart_keyboard(struct rm_pogo_data *pdata) +static __always_inline void +fsm_routine_uart_keyboard_handle_message(struct rm_pogo_data *pdata, + const u8 msg[ONE_WIRE_MCU_MSG_SIZE], + const int count, int len) { - int count, ret, len, len_max; - u8 msg[ONE_WIRE_MCU_MSG_SIZE]; - - if (!(pdata->app_session & APP_SESSION_UART)) - return; + int ret; + int len_max; - GET_AND_VERIFY_MSG(msg, ONE_WIRE_MCU_MSG_SIZE, , false); + dev_dbg(pdata->dev, + "%s: handling message with count %d and payload len %d\n", + __func__, count, len); switch (msg[2]) { case KB_REPORT_KEY: mod_timer(&pdata->alive_timer, jiffies + msecs_to_jiffies(pdata->alive_timeout)); - dev_dbg(pdata->dev, "report key val 0x%x\n", msg[3]); pogo_keyboard_report(pdata, msg[3]); - return; + break; case KB_REPORT_ALIVE: mod_timer(&pdata->alive_timer, jiffies + msecs_to_jiffies(pdata->alive_timeout)); if (pdata->user_command != POGO_CMD_NONE) { memset(pdata->user_command_response, 0x00, - sizeof(pdata->user_command_response)); + sizeof(pdata->user_command_response)); /* This will work also if there is no data to send. * In that case pdata->user_command == NULL and * pdata->user_command_data_len == 0 */ @@ -750,11 +776,10 @@ static void fsm_routine_uart_keyboard(struct rm_pogo_data *pdata) "sending user cmd 0x%x with data of length %d\n", pdata->user_command, pdata->user_command_data_len); - ret = pogo_onewire_write( - pdata, pdata->user_command, 0, - pdata->user_command_data, - pdata->user_command_data_len, - true); + ret = pogo_onewire_write(pdata, pdata->user_command, 0, + pdata->user_command_data, + pdata->user_command_data_len, + true); if (ret != 0) { dev_err(pdata->dev, "failed sending user cmd 0x%x\n", @@ -763,7 +788,7 @@ static void fsm_routine_uart_keyboard(struct rm_pogo_data *pdata) } mod_timer(&pdata->alive_timer, jiffies + msecs_to_jiffies(pdata->alive_timeout)); - return; + break; case POGO_CMD_ATTRIBUTE_READ: len = packet_payload_len(msg); update_pdata_from_attribute_read_response(pdata, &msg[3], len); @@ -787,14 +812,11 @@ static void fsm_routine_uart_keyboard(struct rm_pogo_data *pdata) pdata->fsm_err = true; break; default: - if (pdata->user_command == POGO_CMD_NONE) { - dev_err(pdata->dev, "Unsupported cmd 0x%x\n", - msg[2]); - return; - } else { - break; - } + if (pdata->user_command == POGO_CMD_NONE) + dev_err(pdata->dev, "Unsupported cmd 0x%x\n", msg[2]); + break; } + /* * We should only get here if the command is anything else than * KB_REPORT_KEY or KB_REPORT_ALIVE as these commands are not allowed @@ -802,12 +824,14 @@ static void fsm_routine_uart_keyboard(struct rm_pogo_data *pdata) */ if (pdata->user_command != POGO_CMD_NONE) { if (msg[2] == pdata->user_command) { - dev_dbg(pdata->dev, "user command reply 0x%x\n", msg[2]); + dev_dbg(pdata->dev, "user command reply 0x%x\n", + msg[2]); len = packet_len(msg); len_max = sizeof(pdata->user_command_response); if (len > len_max) { - dev_warn(pdata->dev, - "user command reply length (%d) exceeded allowed limit (%d)\n", + dev_warn( + pdata->dev, + "user command reply length (%d) exceeded allowed limit (%d)\n", len, len_max); /* * We choose not to abort here but to limit the output data to what the @@ -819,7 +843,7 @@ static void fsm_routine_uart_keyboard(struct rm_pogo_data *pdata) } else { dev_dbg(pdata->dev, "wrong user command reply 0x%x, expected 0x%x\n", - msg[2], pdata->user_command); + msg[2], pdata->user_command); } pdata->user_command = POGO_CMD_NONE; if (pdata->user_command_data) { @@ -829,6 +853,29 @@ static void fsm_routine_uart_keyboard(struct rm_pogo_data *pdata) } } +static void fsm_routine_uart_keyboard(struct rm_pogo_data *pdata) +{ + int count, len; + u8 msg[ONE_WIRE_MCU_MSG_SIZE]; + + if (!(pdata->app_session & APP_SESSION_UART)) + return; + + dev_dbg(pdata->dev, "%s: handling RX uart data\n", __func__); + + GET_AND_VERIFY_MSG(msg, ONE_WIRE_MCU_MSG_SIZE, , false); + + while (count > 0) { + fsm_routine_uart_keyboard_handle_message(pdata, msg, count, + len); + + /* Do we have more data in the RX buffer? + * If not, that's fine, the macro will return from this + * function. */ + GET_AND_VERIFY_MSG(msg, ONE_WIRE_MCU_MSG_SIZE, , false); + } +} + static bool fsm_entry_usb_keyboard(struct rm_pogo_data *pdata) { /* Not implemented */ diff --git a/drivers/misc/rm-pogo/pogo_kpad.c b/drivers/misc/rm-pogo/pogo_kpad.c index ae8cfd23231f..e7d16b290cfc 100644 --- a/drivers/misc/rm-pogo/pogo_kpad.c +++ b/drivers/misc/rm-pogo/pogo_kpad.c @@ -574,8 +574,8 @@ void pogo_keyboard_report(struct rm_pogo_data *pdata, u8 val) row = 0x7 & (val >> 1); col = 0xf & (val >> 4); key = MATRIX_SCAN_CODE(row, col, pdata->kb_row_shift); - dev_dbg(pdata->dev, "Report row %d col %d key_idx %d code %d\n", - row, col, key, keycodes[key]); + dev_dbg(pdata->dev, "Report row %d col %d key_idx %d code %d active %d\n", + row, col, key, keycodes[key], val & 1); input_report_key(pdata->kb_dev, keycodes[key], val & 1); input_sync(pdata->kb_dev); } diff --git a/drivers/misc/rm-pogo/pogo_main.c b/drivers/misc/rm-pogo/pogo_main.c index ea78b5ee1323..c2136adb19fc 100644 --- a/drivers/misc/rm-pogo/pogo_main.c +++ b/drivers/misc/rm-pogo/pogo_main.c @@ -239,14 +239,62 @@ int __must_check pogo_onewire_write(struct rm_pogo_data *data, u8 cmd, u8 ext, return -EIO; } +/* + * Handle one message. Only used in receive_buf. + * + * return -EINVAL on parsing error + * return -EAGAIN when we haven't received enough bytes given packet header len + */ +static __always_inline int pogo_onewire_receive_buf_handle_message( + struct rm_pogo_data *data, const unsigned char *buf, const size_t count) +{ + int i = 0; + unsigned int len = 0; + uint8_t checksum = 0; + uint8_t rcv_check = 0; + + /* Follow Accessory interface description/specification: + * start_1 : data_len_1 : cmd_2 : datax : checksum_from_len_1 : end_1 + */ + if (buf[0] != '.') { /* check magic */ + dev_warn(data->dev, "invalid magic 0x%2x\n", buf[0]); + return -EINVAL; + } + + len = buf[1] | ((0xf & buf[2]) << 8); + if (5 + len > count) { + dev_warn(data->dev, "%d bytes ready, wait %d bytes\n", count, + 5 + len); + return -EAGAIN; + } + + checksum = buf[1] + buf[2] + buf[3]; /* add len, cmd */ + + for (i = 0; i < len; i++) { + checksum += buf[4 + i]; + } + rcv_check = buf[4 + len]; + + checksum += rcv_check; + if (checksum) { + dev_warn(data->dev, "rx packet checksum error\n"); + return -EINVAL; + } + + // SUCCESS + return len; +} + static int pogo_onewire_receive_buf(struct serdev_device *serdev, - const unsigned char *buf, size_t count) + const unsigned char *buf, const size_t count) { struct rm_pogo_data *data = serdev_device_get_drvdata(serdev); int i, ret = 0; - u16 len; - u8 rcv_check = 0, checksum = 0; + int len = 0; + int bytes_processed = 0; + const bool tx_ack_required_backup = data->tx_ack_required; + // FIXME Remove, only for debugging unsigned char *str_buf = kzalloc((ONE_WIRE_MAX_TX_MSG_SIZE * 2) + 1, GFP_KERNEL); @@ -275,7 +323,7 @@ static int pogo_onewire_receive_buf(struct serdev_device *serdev, return 0; } - dev_dbg(data->dev, "%d bytes packet received.\n", count); + dev_dbg(data->dev, "%s: %d bytes data received.\n", __func__, count); for (i = 0; i < count; ++i) { snprintf(str_buf + (i * 2), 3, "%02hhx", buf[i]); @@ -283,55 +331,67 @@ static int pogo_onewire_receive_buf(struct serdev_device *serdev, dev_dbg(data->dev, "%s: complete recv msg: %s\n", __func__, str_buf); kfree(str_buf); + /* For debugging from user-space */ if (count <= ONE_WIRE_MAX_TX_MSG_SIZE) { memset(data->onewire_rx_buf, 0, ONE_WIRE_MAX_TX_MSG_SIZE); memcpy(data->onewire_rx_buf, buf, count); data->onewire_rx_buf_len = count; } - /* Follow Accessory interface description/specification: - * start_1 : data_len_1 : cmd_2 : datax : checksum_from_len_1 : end_1 + /* There may be multiple messages in the RX buffer. + * Parse one at a time, signal FSM thread once after. */ - if (buf[0] != '.') { /* check magic */ - dev_warn(data->dev, - "invalid magic 0x%2x\n", buf[0]); - if (data->tx_ack_required) - goto resend_unlock; - else + while (count > bytes_processed) { + ret = pogo_onewire_receive_buf_handle_message( + data, buf + bytes_processed, count - bytes_processed); + + if (ret == -EINVAL) { + /* We've received a buffer we can't parse. We don't + * want to get the same buffer contents again, so mark + * all remaining as processed and drop. + */ + bytes_processed = count; + if (data->tx_ack_required) + goto resend_unlock; + else + goto ignore_rx; + + } else if (ret == -EAGAIN) { + /* We are waiting for more data. Don't mark this + * message as processed. */ + goto rx_unlock; + } else if (ret < 0) { + dev_warn(data->dev, + "%s: unknown error %d, ignoring RX\n", + __func__, ret); + bytes_processed = count; goto ignore_rx; - } - - len = buf[1] | ((0xf & buf[2]) << 8); - if (5 + len > count) { - dev_warn(data->dev, "%d bytes ready, wait %d bytes\n", - count, 5 + len); - count = 0; - goto rx_unlock; - } + } - checksum = buf[1] + buf[2] + buf[3]; /* add len, cmd */ + // ret is now the payload len of the most recent packet + len = ret; - for (i = 0; i < len; i++) { - checksum += buf[4 + i]; - } - rcv_check = buf[4 + len]; - - checksum += rcv_check; - if (checksum) { - dev_warn(data->dev, - "rx packet checksum error\n"); - if (data->tx_ack_required) - goto resend_unlock; - else - goto ignore_rx; + /* Do we have enough buffer space available? */ + if (kfifo_avail(&data->read_fifo) < (3 + len)) { + dev_warn( + data->dev, + "%s: warning, receiving data faster than we can handle! overflow of %d bytes\n", + __func__, + (3 + len) - kfifo_avail(&data->read_fifo)); + // We abort. + break; + } else { + /* send len, cmd and data to FSM layer */ + ret = kfifo_in(&data->read_fifo, + (buf + bytes_processed) + 1, 3 + len); + dev_dbg(data->dev, "%s: processed %d (len %d)\n", + __func__, 5 + len, len); + + /* Set processed to what's actually processed, so magic + * byte, header, payload and CRC. */ + bytes_processed += (5 + len); + } } - - /* send len, cmd and data to FSM layer */ - ret = kfifo_in(&data->read_fifo, buf + 1, 3 + len); - dev_dbg(data->dev, "%s: %d bytes message received, wake up FSM..\n", - __func__, len + 3); - - wake_up_process(data->fsm_thread); goto rx_unlock; resend_unlock: @@ -356,15 +416,22 @@ static int pogo_onewire_receive_buf(struct serdev_device *serdev, } rx_unlock: + if (bytes_processed > 0) { + dev_dbg(data->dev, + "%s: %d RX bytes processed. Waking up FSM...\n", + __func__, bytes_processed); + wake_up_process(data->fsm_thread); + } + mutex_unlock(&data->lock); /* report the consumed data so those will not be reported again */ - return count; + return bytes_processed; ignore_rx: kfifo_reset(&data->read_fifo); mutex_unlock(&data->lock); - return count; + return bytes_processed; } static const struct serdev_device_ops pogo_serdev_ops = { @@ -490,7 +557,8 @@ static int rm_pogo_probe(struct serdev_device *serdev) pdata->pogo_name = NULL; ret = kfifo_alloc(&pdata->read_fifo, - ONE_WIRE_MCU_MSG_SIZE, GFP_KERNEL); + ONE_WIRE_MCU_MSG_SIZE * NUM_MSGS_RECV_BUFFER, + GFP_KERNEL); if (ret) goto error_1;