From 27ecf6b352f7063579a67329c8e73961d7103f02 Mon Sep 17 00:00:00 2001 From: Kyle Deng Date: Fri, 21 Nov 2025 16:13:11 +0800 Subject: [PATCH 1/2] diag: Added queue push using scatter-gather for frame build Introduce a new queue push mechanism to build NHDLC frames and enqueue them via the scatter-gather push function. This approach reduces memory copies and improves performance for framed message transmission. Signed-off-by: Kyle Deng --- router/diag.c | 48 ++++++++++++++++++++++++++++++++++++++++++++++++ router/diag.h | 1 + 2 files changed, 49 insertions(+) diff --git a/router/diag.c b/router/diag.c index b5e443d..e311ebe 100644 --- a/router/diag.c +++ b/router/diag.c @@ -47,6 +47,54 @@ struct list_head diag_cmds = LIST_INIT(diag_cmds); +static size_t iovec_total_len(struct iovec *iov, int iovcnt) +{ + size_t total = 0; + + for (int i = 0; i < iovcnt; i++) { + total += iov[i].iov_len; + } + + return total; +} + +void queue_push_sg_flow(struct list_head *queue, struct iovec *iov, int iovcnt, + struct watch_flow *flow) +{ + struct mbuf *mbuf; + void *ptr, *src; + size_t iovec_len, len; + size_t offset = 0; + + iovec_len = iovec_total_len(iov, iovcnt); + mbuf = mbuf_alloc(sizeof(*mbuf) + iovec_len); + if (!mbuf) { + warnx("Diag: %s: failed to allocate memory", __func__); + return; + } + + ptr = mbuf_put(mbuf, sizeof(*mbuf) + iovec_len); + if (!ptr) { + warnx("Diag: invalid ptr, dropping pkt of len: %zu\n", sizeof(*mbuf) + iovec_len); + free(mbuf); + return; + } + + for (int i = 0; i < iovcnt; ++i) { + src = iov[i].iov_base; + len = iov[i].iov_len; + memcpy(ptr + offset, src, len); + offset += len; + } + + mbuf->offset = offset; + mbuf->size = iovec_len; + mbuf->flow = flow; + + watch_flow_inc(flow); + list_add(queue, &mbuf->node); +} + void queue_push_flow(struct list_head *queue, const void *msg, size_t msglen, struct watch_flow *flow) { diff --git a/router/diag.h b/router/diag.h index 19270bf..13265b7 100644 --- a/router/diag.h +++ b/router/diag.h @@ -34,6 +34,7 @@ #include #include +#include #include "circ_buf.h" #include "hdlc.h" From b22ec9ccd062e40fd336805fa562ba9469002cb5 Mon Sep 17 00:00:00 2001 From: Kyle Deng Date: Fri, 21 Nov 2025 16:21:01 +0800 Subject: [PATCH 2/2] diag-router: Add support for non-HDLC mode MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Added the support of NHDLC mode.DIAG packets using the newer non‑HDLC framing format instead of full HDLC encoding to reduce overhead and redundancy on reliable transports like USB. Signed-off-by: Kyle Deng --- router/app_cmds.c | 32 ++++++++++ router/diag.c | 23 +++++++ router/diag.h | 11 ++++ router/dm.c | 160 +++++++++++++++++++++++++++++++++++++--------- router/dm.h | 11 +++- router/usb.c | 19 +----- 6 files changed, 207 insertions(+), 49 deletions(-) diff --git a/router/app_cmds.c b/router/app_cmds.c index cf42a86..d819095 100644 --- a/router/app_cmds.c +++ b/router/app_cmds.c @@ -54,6 +54,8 @@ #define MSM_REVISION_NUMBER 2 #define DIAG_CMD_DIAG_SUBSYS 18 + +#define DIAG_CMD_OP_HDLC_DISABLE 0x218 #define DIAG_CMD_DIAG_GET_DIAG_ID 0x222 static int handle_diag_version(struct diag_client *client, const void *buf, @@ -163,6 +165,34 @@ static int handle_diag_id(struct diag_client *client, const void *buf, size_t le return dm_send(client, resp_buffer, resp_len); } +static int handle_hdlc_disable_cmd(struct diag_client *client, const void *buf, size_t len) +{ + struct hdlc_disable_req { + uint8_t cmd_code; + uint8_t subsys_id; + uint16_t subsys_cmd_code; + } __packed; + struct hdlc_disable_resp { + struct hdlc_disable_req req_info; + uint8_t version; + uint8_t result; + } __packed; + struct hdlc_disable_req *req = (struct hdlc_disable_req *)buf; + struct hdlc_disable_resp resp = {0}; + int ret; + + if (!buf || len < sizeof(*req)) + return -EMSGSIZE; + + memcpy(&resp.req_info, req, sizeof(*req)); + resp.version = 1; + resp.result = 0; + ret = dm_send(client, (unsigned char *)&resp, sizeof(resp)); + + set_encode_type(DIAG_ENCODE_NHDLC); + return ret; +} + void register_app_cmds(void) { register_fallback_cmd(DIAG_CMD_DIAG_VERSION_ID, handle_diag_version); @@ -172,4 +202,6 @@ void register_app_cmds(void) DIAG_CMD_KEEP_ALIVE_CMD, handle_keep_alive); register_fallback_subsys_cmd(DIAG_CMD_DIAG_SUBSYS, DIAG_CMD_DIAG_GET_DIAG_ID, handle_diag_id); + register_fallback_subsys_cmd(DIAG_CMD_DIAG_SUBSYS, + DIAG_CMD_OP_HDLC_DISABLE, handle_hdlc_disable_cmd); } diff --git a/router/diag.c b/router/diag.c index e311ebe..61daf42 100644 --- a/router/diag.c +++ b/router/diag.c @@ -95,6 +95,29 @@ void queue_push_sg_flow(struct list_head *queue, struct iovec *iov, int iovcnt, list_add(queue, &mbuf->node); } +int nhdlc_enqueue_flow(struct list_head *queue, const void *msg, size_t msglen, + struct watch_flow *flow) +{ + struct diag_pkt_frame header = {0}; + uint8_t tail = NHDLC_CONTROL_CHAR; + struct iovec iov[3]; + + header.start = NHDLC_CONTROL_CHAR; + header.version = 1; + header.length = msglen; + + iov[0].iov_base = &header; + iov[0].iov_len = sizeof(header); + iov[1].iov_base = msg; + iov[1].iov_len = msglen; + iov[2].iov_base = &tail; + iov[2].iov_len = sizeof(uint8_t); + + queue_push_sg_flow(queue, iov, sizeof(iov) / sizeof(iov[0]), flow); + + return 0; +} + void queue_push_flow(struct list_head *queue, const void *msg, size_t msglen, struct watch_flow *flow) { diff --git a/router/diag.h b/router/diag.h index 13265b7..3b093db 100644 --- a/router/diag.h +++ b/router/diag.h @@ -65,6 +65,8 @@ #define DIAG_CMD_SUBSYS_DISPATCH 75 #define DIAG_CMD_SUBSYS_DISPATCH_V2 128 +#define NHDLC_CONTROL_CHAR 0x7E + struct diag_client; struct peripheral { @@ -110,6 +112,13 @@ struct diag_cmd { int(*cb)(struct diag_client *client, const void *buf, size_t len); }; +struct diag_pkt_frame { + uint8_t start; + uint8_t version; + uint16_t length; + unsigned char data[0]; +} __packed; + void queue_push(struct list_head *queue, const void *msg, size_t msglen); void queue_push_flow(struct list_head *queue, const void *msg, size_t msglen, struct watch_flow *flow); @@ -126,6 +135,8 @@ int diag_client_handle_command(struct diag_client *client, uint8_t *data, size_t int hdlc_enqueue(struct list_head *queue, const void *buf, size_t msglen); int hdlc_enqueue_flow(struct list_head *queue, const void *buf, size_t msglen, struct watch_flow *flow); +int nhdlc_enqueue_flow(struct list_head *queue, const void *msg, size_t msglen, + struct watch_flow *flow); void register_fallback_cmd(unsigned int cmd, int(*cb)(struct diag_client *client, diff --git a/router/dm.c b/router/dm.c index b669bef..91d3616 100644 --- a/router/dm.c +++ b/router/dm.c @@ -50,7 +50,8 @@ struct diag_client { int in_fd; int out_fd; - bool hdlc_encoded; + int encode_type; + bool encode_hdlc_reset; bool enabled; @@ -67,7 +68,7 @@ struct list_head diag_clients = LIST_INIT(diag_clients); * dm_add() - register new DM * @dm: DM object to register */ -struct diag_client *dm_add(const char *name, int in_fd, int out_fd, bool hdlc_encoded) +struct diag_client *dm_add(const char *name, int in_fd, int out_fd, bool is_encoded) { struct diag_client *dm; @@ -78,7 +79,7 @@ struct diag_client *dm_add(const char *name, int in_fd, int out_fd, bool hdlc_en dm->name = strdup(name); dm->in_fd = in_fd; dm->out_fd = out_fd; - dm->hdlc_encoded = hdlc_encoded; + dm->encode_type = (is_encoded) ? DIAG_ENCODE_HDLC : DIAG_ENCODE_RAW; list_init(&dm->outq); if (dm->in_fd >= 0) @@ -93,34 +94,21 @@ struct diag_client *dm_add(const char *name, int in_fd, int out_fd, bool hdlc_en return dm; } -static int dm_recv_hdlc(struct diag_client *dm) +static int dm_recv_hdlc(struct diag_client *dm, struct circ_buf *buf) { + struct hdlc_decoder recv_decoder = {0}; size_t msglen; - ssize_t n; void *msg; - int ret = 0; for (;;) { - n = circ_read(dm->in_fd, &dm->recv_buf); - if (n < 0 && errno == EAGAIN) { - break; - } else if (n < 0) { - ret = -errno; - warn("Failed to read from %s\n", dm->name); + msg = hdlc_decode_one(&recv_decoder, buf, &msglen); + if (!msg) break; - } - - for (;;) { - msg = hdlc_decode_one(&dm->recv_decoder, &dm->recv_buf, - &msglen); - if (!msg) - break; - diag_client_handle_command(dm, msg, msglen); - } + diag_client_handle_command(dm, msg, msglen); } - return ret; + return 0; } static int dm_recv_raw(struct diag_client *dm) @@ -148,6 +136,84 @@ static int dm_recv_raw(struct diag_client *dm) return 0; } +static void diag_hdlc_reset(void *data) +{ + struct diag_client *dm = (struct diag_client *)data; + + set_encode_type(DIAG_ENCODE_HDLC); + dm->encode_hdlc_reset = false; +} + +#define DIAG_MAX_BAD_CMD 5 +static int diag_start_hdlc_recovery(struct diag_client *dm) +{ + static uint32_t bad_cmd_counter; + + if (!dm->encode_hdlc_reset) { + watch_add_timer(diag_hdlc_reset, (void *)dm, 200, 0); + dm->encode_hdlc_reset = true; + } + + bad_cmd_counter++; + if (bad_cmd_counter > DIAG_MAX_BAD_CMD) { + bad_cmd_counter = 0; + dm->encode_type = DIAG_ENCODE_HDLC; + } + + return 0; +} + +static int dm_check_nhdlc_pkt(struct diag_client *dm, struct diag_pkt_frame *pkt_ptr) +{ + if (pkt_ptr->start != NHDLC_CONTROL_CHAR || + *(uint8_t *)(pkt_ptr->data + pkt_ptr->length) != NHDLC_CONTROL_CHAR) { + warn("Diag: pkt is not correct, %s\n", dm->name); + return -EINVAL; + } + + return 0; +} + +static int dm_recv_nhdlc(struct diag_client *dm, struct circ_buf *buf) +{ + struct diag_pkt_frame *pkt_ptr; + int ret; + + pkt_ptr = (struct diag_pkt_frame *)buf; + ret = dm_check_nhdlc_pkt(dm, pkt_ptr); + if (ret) + diag_start_hdlc_recovery(dm); + diag_client_handle_command(dm, &pkt_ptr->data[0], pkt_ptr->length); + + return ret; +} + +int dm_decode_data(struct diag_client *dm, struct circ_buf *buf) +{ + switch (dm->encode_type) { + case DIAG_ENCODE_HDLC: + return dm_recv_hdlc(dm, buf); + case DIAG_ENCODE_NHDLC: + return dm_recv_nhdlc(dm, buf); + default: + warn("Diag: recv error encode type %d\n", dm->encode_type); + return -EINVAL; + } +} + +static int dm_recv_encoded(struct diag_client *dm) +{ + ssize_t n; + + n = circ_read(dm->in_fd, &dm->recv_buf); + if (n < 0 && errno != EAGAIN) { + warn("Failed to read from %s\n", dm->name); + return -errno; + } + + return dm_decode_data(dm, &dm->recv_buf); +} + /** * dm_recv() - read and handle data from a DM * @fd: the file descriptor associated with the DM @@ -157,22 +223,43 @@ int dm_recv(int fd, void* data) { struct diag_client *dm = (struct diag_client *)data; - if (dm->hdlc_encoded) - return dm_recv_hdlc(dm); - else + if (!dm) + return -EINVAL; + + switch (dm->encode_type) { + case DIAG_ENCODE_RAW: return dm_recv_raw(dm); + case DIAG_ENCODE_HDLC: + case DIAG_ENCODE_NHDLC: + return dm_recv_encoded(dm); + default: + warn("Diag: recv error encode type %d\n", dm->encode_type); + break; + } + + return -EINVAL; } -static ssize_t dm_send_flow(struct diag_client *dm, const void *ptr, size_t len, +static int dm_send_flow(struct diag_client *dm, const void *ptr, size_t len, struct watch_flow *flow) { - if (!dm->enabled) + if (dm && !dm->enabled) return 0; - if (dm->hdlc_encoded) - hdlc_enqueue_flow(&dm->outq, ptr, len, flow); - else + switch (dm->encode_type) { + case DIAG_ENCODE_RAW: queue_push_flow(&dm->outq, ptr, len, flow); + break; + case DIAG_ENCODE_HDLC: + hdlc_enqueue_flow(&dm->outq, ptr, len, flow); + break; + case DIAG_ENCODE_NHDLC: + nhdlc_enqueue_flow(&dm->outq, ptr, len, flow); + break; + default: + warn("Diag: send error encode type %d\n", dm->encode_type); + return -EINVAL; + } return 0; } @@ -183,7 +270,7 @@ static ssize_t dm_send_flow(struct diag_client *dm, const void *ptr, size_t len, * @ptr: pointer to raw message to be sent * @len: length of message */ -ssize_t dm_send(struct diag_client *dm, const void *ptr, size_t len) +int dm_send(struct diag_client *dm, const void *ptr, size_t len) { return dm_send_flow(dm, ptr, len, NULL); } @@ -217,3 +304,14 @@ void dm_disable(struct diag_client *dm) /* XXX: purge dm->outq */ } + +void set_encode_type(int type) +{ + struct diag_client *dm = NULL; + + list_for_each_entry(dm, &diag_clients, node) { + if (dm->encode_type == DIAG_ENCODE_RAW) + continue; + dm->encode_type = type; + } +} diff --git a/router/dm.h b/router/dm.h index 9c2195c..6dc4c9a 100644 --- a/router/dm.h +++ b/router/dm.h @@ -34,14 +34,23 @@ #include "diag.h" +enum diag_encode_type { + DIAG_ENCODE_RAW = 1, + DIAG_ENCODE_HDLC, + DIAG_ENCODE_NHDLC, +}; + struct diag_client; struct diag_client *dm_add(const char *name, int in_fd, int out_fd, bool hdlc_encoded); int dm_recv(int fd, void* data); -ssize_t dm_send(struct diag_client *dm, const void *ptr, size_t len); +int dm_send(struct diag_client *dm, const void *ptr, size_t len); void dm_broadcast(const void *ptr, size_t len, struct watch_flow *flow); void dm_enable(struct diag_client *dm); void dm_disable(struct diag_client *dm); +int dm_decode_data(struct diag_client *dm, struct circ_buf *buf); +void set_encode_type(int type); + #endif diff --git a/router/usb.c b/router/usb.c index 34ded0e..b0eddc8 100644 --- a/router/usb.c +++ b/router/usb.c @@ -278,29 +278,14 @@ static int ffs_diag_init(const char *ffs_name, struct usb_handle *h) static int diag_ffs_recv(struct mbuf *mbuf, void *data) { - struct hdlc_decoder recv_decoder; - struct circ_buf recv_buf; + struct circ_buf recv_buf = {0}; struct usb_handle *ffs = data; - size_t msglen; - void *msg; - - memset(&recv_decoder, 0, sizeof(recv_decoder)); memcpy(recv_buf.buf, mbuf->data, mbuf->offset); recv_buf.tail = 0; recv_buf.head = mbuf->offset; - // print_hex_dump("[USB]", mbuf->data, mbuf->offset); - - for (;;) { - msg = hdlc_decode_one(&recv_decoder, &recv_buf, &msglen); - if (!msg) - break; - - // print_hex_dump(" [MSG]", msg, MIN(msglen, 256)); - - diag_client_handle_command(ffs->dm, msg, msglen); - } + dm_decode_data(ffs->dm, &recv_buf); mbuf->offset = 0; list_add(&ffs->outq, &mbuf->node);