From 221ae75980ae733880f6c5d03b2187c5cc02d1ab Mon Sep 17 00:00:00 2001 From: Johan Hedberg Date: Wed, 27 Apr 2016 14:03:35 +0300 Subject: [PATCH] monitor: Update to new header format of TTY protocol The updated TTY protocol content looks like: uint16 data_len uint16 opcode uint8 flags uint8 hdr_len uint8 extra_hdr[hdr_len]; uint8 payload[data_len - 4 - hdr_len] --- monitor/control.c | 44 +++++++++++++++++++++++++++++++------------- 1 file 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); } } -- 2.47.3