From e12dfabdf00eef39acacbe92933ce58d7b8e621d Mon Sep 17 00:00:00 2001 From: Caleb Connolly Date: Mon, 18 Jul 2022 11:38:35 +0100 Subject: [PATCH] Add support for the qmi_tlv based QMI accessor --- Makefile | 1 + lib/libqrtr.h | 15 +++ lib/logging.h | 2 +- lib/qmi_tlv.c | 276 ++++++++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 293 insertions(+), 1 deletion(-) create mode 100644 lib/qmi_tlv.c diff --git a/Makefile b/Makefile index 036691c..ec8c2e0 100644 --- a/Makefile +++ b/Makefile @@ -44,6 +44,7 @@ $(proj)-lookup-cflags := -Ilib lib$(proj).so-srcs := \ lib/logging.c \ + lib/qmi_tlv.c \ lib/qrtr.c \ lib/qmi.c diff --git a/lib/libqrtr.h b/lib/libqrtr.h index 2a34c1c..10929b0 100644 --- a/lib/libqrtr.h +++ b/lib/libqrtr.h @@ -25,6 +25,7 @@ extern "C" { #endif struct sockaddr_qrtr; +struct qmi_tlv; struct qrtr_packet { int type; @@ -166,6 +167,20 @@ ssize_t qmi_encode_message(struct qrtr_packet *pkt, int type, int msg_id, int txn_id, const void *c_struct, struct qmi_elem_info *ei); +struct qmi_tlv *qmi_tlv_init(uint16_t txn, uint32_t msg_id, uint32_t msg_type); +void *qmi_tlv_encode(struct qmi_tlv *tlv, size_t *len); +struct qmi_tlv *qmi_tlv_decode(void *buf, size_t len, uint16_t *txn); +void qmi_tlv_free(struct qmi_tlv *tlv); +void qmi_tlv_dump(struct qmi_tlv *tlv); +void *qmi_tlv_get(struct qmi_tlv *tlv, uint8_t id, size_t *len); +void *qmi_tlv_get_array(struct qmi_tlv *tlv, uint8_t id, size_t len_size, + size_t *len, size_t *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); + /* Initial kernel header didn't expose these */ #ifndef QRTR_NODE_BCAST diff --git a/lib/logging.h b/lib/logging.h index a053dce..c87a7b1 100644 --- a/lib/logging.h +++ b/lib/logging.h @@ -38,7 +38,7 @@ void qlog(int priority, const char *format, ...) __PRINTF__(2, 3); } while(0) #ifdef __cplusplus -extern "C" { +} #endif #endif diff --git a/lib/qmi_tlv.c b/lib/qmi_tlv.c new file mode 100644 index 0000000..6fe6116 --- /dev/null +++ b/lib/qmi_tlv.c @@ -0,0 +1,276 @@ +#include +#include +#include +#include +#include +#include + +#include "libqrtr.h" + +struct qmi_tlv { + void *allocated; + void *buf; + size_t size; + int error; +}; + +struct qmi_tlv_item { + uint8_t key; + uint16_t len; + uint8_t data[]; +} __attribute__((__packed__)); + +struct qmi_tlv *qmi_tlv_init(uint16_t txn, uint32_t msg_id, uint32_t msg_type) +{ + struct qmi_header *pkt; + struct qmi_tlv *tlv; + + tlv = malloc(sizeof(struct qmi_tlv)); + memset(tlv, 0, sizeof(struct qmi_tlv)); + + tlv->size = sizeof(struct qmi_header) + 1; + tlv->allocated = malloc(tlv->size); + tlv->buf = tlv->allocated; + + pkt = tlv->buf; + pkt->type = msg_type; + pkt->txn_id = txn; + pkt->msg_id = msg_id; + pkt->msg_len = 0; + + return tlv; +} + +struct qmi_tlv *qmi_tlv_decode(void *buf, size_t len, uint16_t *txn) +{ + struct qmi_header *pkt = buf; + struct qmi_tlv *tlv; + + tlv = malloc(sizeof(struct qmi_tlv)); + memset(tlv, 0, sizeof(struct qmi_tlv)); + + tlv->buf = buf; + tlv->size = len; + + if (txn) + *txn = pkt->txn_id; + + return tlv; +} + +void *qmi_tlv_encode(struct qmi_tlv *tlv, size_t *len) +{ + struct qmi_header *pkt; + + if (!tlv || tlv->error) + return NULL; + + pkt = tlv->buf; + pkt->msg_len = tlv->size - sizeof(struct qmi_header); + + *len = tlv->size; + return tlv->buf; +} + +void qmi_tlv_free(struct qmi_tlv *tlv) +{ + free(tlv->allocated); + free(tlv); +} + +static struct qmi_tlv_item *qmi_tlv_get_item(struct qmi_tlv *tlv, unsigned id) +{ + struct qmi_tlv_item *item; + struct qmi_header *pkt; + unsigned offset = 0; + void *pkt_data; + + pkt = tlv->buf; + pkt_data = &pkt[1]; + + while (offset < tlv->size) { + item = pkt_data + offset; + if (item->key == id) + return pkt_data + offset; + + offset += sizeof(struct qmi_tlv_item) + item->len; + } + return NULL; +} + +void *qmi_tlv_get(struct qmi_tlv *tlv, uint8_t id, size_t *len) +{ + struct qmi_tlv_item *item; + + item = qmi_tlv_get_item(tlv, id); + if (!item) + return NULL; + + if (len) + *len = item->len; + return item->data; +} + +void *qmi_tlv_get_array(struct qmi_tlv *tlv, uint8_t id, size_t len_size, + size_t *len, size_t *size) +{ + struct qmi_tlv_item *item; + unsigned count; + void *ptr; + + item = qmi_tlv_get_item(tlv, id); + if (!item) + return NULL; + + ptr = item->data; + switch (len_size) { + case 4: + count = *(uint32_t *)ptr++; + break; + case 2: + count = *(uint16_t *)ptr++; + break; + case 1: + count = *(uint8_t *)ptr++; + break; + default: + return NULL; + } + + *len = count; + *size = (item->len - len_size) / count; + + return ptr; +} + +static struct qmi_tlv_item *qmi_tlv_alloc_item(struct qmi_tlv *tlv, unsigned id, + size_t len) +{ + struct qmi_tlv_item *item; + size_t new_size; + bool migrate; + void *newp; + + /* If using user provided buffer, migrate data */ + migrate = !tlv->allocated; + + new_size = tlv->size + sizeof(struct qmi_tlv_item) + len; + newp = realloc(tlv->allocated, new_size); + if (!newp) + return NULL; + + if (migrate) + memcpy(newp, tlv->buf, tlv->size); + + item = newp + tlv->size; + item->key = id; + item->len = len; + + tlv->buf = tlv->allocated = newp; + tlv->size = new_size; + + return item; +} + +int qmi_tlv_set(struct qmi_tlv *tlv, uint8_t id, void *buf, size_t len) +{ + struct qmi_tlv_item *item; + + if (!tlv) + return -EINVAL; + + item = qmi_tlv_alloc_item(tlv, id, len); + if (!item) { + tlv->error = ENOMEM; + return -ENOMEM; + } + + memcpy(item->data, buf, len); + + return 0; +} + +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_tlv_item *item; + size_t array_size; + void *ptr; + + if (!tlv) + return -EINVAL; + + array_size = len * size; + item = qmi_tlv_alloc_item(tlv, id, len_size + array_size); + if (!item) { + tlv->error = ENOMEM; + return -ENOMEM; + } + + ptr = item->data; + + switch (len_size) { + case 4: + *(uint32_t *)ptr++ = len; + break; + case 2: + *(uint16_t *)ptr++ = len; + break; + case 1: + *(uint8_t *)ptr++ = len; + break; + } + memcpy(ptr, buf, array_size); + + return 0; +} + +struct qmi_response_type_v01 *qmi_tlv_get_result(struct qmi_tlv *tlv) +{ + return qmi_tlv_get(tlv, 2, NULL); +} + +#define MIN(x, y) ((x) < (y) ? (x) : (y)) +#define LINE_LENGTH 64 + +static inline uint8_t to_hex(uint8_t ch) +{ + ch &= 0xf; + return ch <= 9 ? '0' + ch : 'a' + ch - 10; +} + +void qmi_tlv_dump(struct qmi_tlv *tlv) { + struct qmi_tlv_item *item; + struct qmi_header *pkt; + unsigned offset = 0; + void *pkt_data; + int i = 0, li, j, k; + uint8_t ch; + size_t linelen; + char line[LINE_LENGTH * 4 + 1]; + + pkt = tlv->buf; + pkt_data = &pkt[1]; + + printf("<<< TLVs:\n"); + while (offset < tlv->size - sizeof(struct qmi_header)) { + item = pkt_data + offset; + printf("<<< TLV %d: {id: 0x%2x, len: 0x%2x}\n", i, item->key, item->len); + for (j = 0; j < item->len; j += LINE_LENGTH) { + linelen = MIN(LINE_LENGTH, item->len - j); + li = 0; + + for (k = 0; k < linelen; k++) { + ch = item->data[j + k]; + line[li++] = to_hex(ch >> 4); + line[li++] = to_hex(ch); + line[li++] = k < linelen - 1 ? ':' : ' '; + } + line[li] = '\0'; + + printf("%s\n\n", line); + } + offset += sizeof(struct qmi_tlv_item) + item->len; + i++; + } +}