diff --git a/lib/libqrtr.h b/lib/libqrtr.h index c644947..e4eaa32 100644 --- a/lib/libqrtr.h +++ b/lib/libqrtr.h @@ -178,9 +178,16 @@ void *qmi_tlv_get_array(struct qmi_tlv *tlv, uint8_t id, size_t len_size, int qmi_tlv_set(struct qmi_tlv *tlv, uint8_t id, void *buf, size_t len); int qmi_tlv_set_array(struct qmi_tlv *tlv, uint8_t id, size_t len_size, void *buf, size_t len, size_t size); - struct qmi_response_type_v01 *qmi_tlv_get_result(struct qmi_tlv *tlv); +static inline int qmi_tlv_dump_buf(void *buf, size_t len) { + struct qmi_tlv *tlv = qmi_tlv_decode(buf, len); + if (!tlv) + return -1; + qmi_tlv_dump(tlv); + qmi_tlv_free(tlv); +} + /* Initial kernel header didn't expose these */ #ifndef QRTR_NODE_BCAST diff --git a/lib/qmi_tlv.c b/lib/qmi_tlv.c index 342a398..a0e6349 100644 --- a/lib/qmi_tlv.c +++ b/lib/qmi_tlv.c @@ -1,3 +1,4 @@ +#include #include #include #include @@ -227,7 +228,7 @@ struct qmi_response_type_v01 *qmi_tlv_get_result(struct qmi_tlv *tlv) } #define MIN(x, y) ((x) < (y) ? (x) : (y)) -#define LINE_LENGTH 64 +#define LINE_LENGTH 40 static inline uint8_t to_hex(uint8_t ch) { @@ -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 (; k < LINE_LENGTH; k++) { + line[li++] = ' '; + line[li++] = ' '; + line[li++] = ' '; + } + + for (k = 0; k < linelen; k++) { + ch = item->data[j + k]; + line[li++] = isprint(ch) ? ch : '.'; + } + line[li] = '\0'; printf("%s\n\n", line);