diff --git a/lib/qmi_tlv.c b/lib/qmi_tlv.c index 342a398..6a5e6e2 100644 --- a/lib/qmi_tlv.c +++ b/lib/qmi_tlv.c @@ -1,3 +1,4 @@ +#include #include #include #include @@ -238,7 +239,7 @@ static inline uint8_t to_hex(uint8_t ch) void qmi_tlv_dump(struct qmi_tlv *tlv) { struct qmi_tlv_item *item; struct qmi_header *pkt; - unsigned offset = 0; + unsigned offset = sizeof(struct qmi_header); void *pkt_data; int i = 0, li, j, k; uint8_t ch; @@ -254,10 +255,11 @@ void qmi_tlv_dump(struct qmi_tlv *tlv) { printf("<<< msg_id : 0x%1$04x (%1$u)\n", pkt->msg_id); printf("<<< txn_id : 0x%1$04x (%1$u)\n", pkt->txn_id); printf("<<< TLVs:\n"); - while (offset < tlv->size - sizeof(struct qmi_header)) { + // I do not understand why this -1 is needed + while (offset < tlv->size - 1) { item = pkt_data + offset; printf("<<< TLV %d: {id: 0x%02x, len: 0x%02x}\n", i, item->key, item->len); - if (item->len > pkt->msg_len - offset) { + if (item->len > pkt->msg_len + sizeof(struct qmi_header) - offset) { fprintf(stderr, "Invalid item length!\n"); return; } @@ -271,6 +273,18 @@ void qmi_tlv_dump(struct qmi_tlv *tlv) { line[li++] = to_hex(ch); line[li++] = k < linelen - 1 ? ':' : ' '; } + + for (; j < LINE_LENGTH; j++) { + line[li++] = ' '; + line[li++] = ' '; + line[li++] = ' '; + } + + for (j = 0; j < linelen; j++) { + ch = item->data[i + j]; + line[li++] = isprint(ch) ? ch : '.'; + } + line[li] = '\0'; printf("%s\n\n", line);