bthost: Fix length calculation for RFCOMM header

This fixes the length calculation for large packets that requires more
than 1 byte.

Signed-off-by: Manika Shrivastava <manika.sh@samsung.com>
Signed-off-by: Ayush Garg <ayush.garg@samsung.com>

diff --git a/emulator/bthost.c b/emulator/bthost.c
index 7d9cacb..ac017ee 100755 (executable)
--- a/emulator/bthost.c
+++ b/emulator/bthost.c
@@ -2372,20 +2372,25 @@ static void rfcomm_uih_recv(struct bthost *bthost, struct btconn *conn,
        uint16_t hdr_len, data_len;
        const void *p;
 
-       if (len < sizeof(*hdr))
+       if (len < sizeof(*hdr)) {
+               bthost_debug(bthost, "RFCOMM UIH: too short");
                return;
+       }
 
        if (RFCOMM_TEST_EA(hdr->length)) {
                data_len = (uint16_t) GET_LEN8(hdr->length);
                hdr_len = sizeof(*hdr);
        } else {
                uint8_t ex_len = *((uint8_t *)(data + sizeof(*hdr)));
-               data_len = ((uint16_t) hdr->length << 8) | ex_len;
+               data_len = GET_LEN16((((uint16_t) ex_len << 8) | hdr->length));
                hdr_len = sizeof(*hdr) + sizeof(uint8_t);
        }
 
-       if (len < hdr_len + data_len)
+       if (len < hdr_len + data_len) {
+               bthost_debug(bthost, "RFCOMM UIH: %u != %u", len,
+                                               hdr_len + data_len);
                return;
+       }
 
        p = data + hdr_len;
 
@@ -2407,6 +2412,8 @@ static void process_rfcomm(struct bthost *bthost, struct btconn *conn,
 {
        const struct rfcomm_hdr *hdr = data;
 
+       bthost_debug(bthost, "RFCOMM data: %u bytes", len);
+
        switch (RFCOMM_GET_TYPE(hdr->control)) {
        case RFCOMM_SABM:
                rfcomm_sabm_recv(bthost, conn, l2conn, data, len);