diff options
Diffstat (limited to 'monitor')
-rw-r--r-- | monitor/control.c | 44 |
1 files changed, 31 insertions, 13 deletions
diff --git a/monitor/control.c b/monitor/control.c index 67cfdfd9b..05faeef49 100644 --- a/monitor/control.c +++ b/monitor/control.c @@ -1163,6 +1163,13 @@ void control_server(const char *path) server_fd = fd; } +struct tty_hdr { + uint16_t data_len; + uint16_t opcode; + uint8_t flags; + uint8_t hdr_len; +} __attribute__ ((packed)); + static void tty_callback(int fd, uint32_t events, void *user_data) { struct control_data *data = user_data; @@ -1180,23 +1187,34 @@ static void tty_callback(int fd, uint32_t events, void *user_data) data->offset += len; - if (data->offset > MGMT_HDR_SIZE) { - struct mgmt_hdr *hdr = (struct mgmt_hdr *) data->buf; - uint16_t pktlen = le16_to_cpu(hdr->len); - - if (data->offset > pktlen + MGMT_HDR_SIZE) { - uint16_t opcode = le16_to_cpu(hdr->opcode); - uint16_t index = le16_to_cpu(hdr->index); + while (data->offset >= sizeof(struct tty_hdr)) { + struct tty_hdr *hdr = (struct tty_hdr *) data->buf; + uint16_t pktlen, opcode, data_len; - packet_monitor(NULL, NULL, index, opcode, - data->buf + MGMT_HDR_SIZE, pktlen); + data_len = le16_to_cpu(hdr->data_len); - data->offset -= pktlen + MGMT_HDR_SIZE; + if (data->offset < 2 + data_len) + return; - if (data->offset > 0) - memmove(data->buf, data->buf + - MGMT_HDR_SIZE + pktlen, data->offset); + if (data->offset < sizeof(*hdr) + hdr->hdr_len) { + fprintf(stderr, "Received corrupted data from TTY\n"); + memmove(data->buf, data->buf + 2 + data_len, + data->offset); + return; } + + opcode = le16_to_cpu(hdr->opcode); + pktlen = data_len - 4 - hdr->hdr_len; + + packet_monitor(NULL, NULL, 0, opcode, + data->buf + sizeof(*hdr) + hdr->hdr_len, + pktlen); + + data->offset -= 2 + data_len; + + if (data->offset > 0) + memmove(data->buf, data->buf + 2 + data_len, + data->offset); } } |