Skip to content

diag-router: Add support for non-HDLC mode #21

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
ChunkaiDeng wants to merge 2 commits into
base: master
Choose a base branch
from
Open

diag-router: Add support for non-HDLC mode #21

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
27ecf6b
diag: Added queue push using scatter-gather for frame build
ChunkaiDeng Nov 21, 2025
b22ec9c
diag-router: Add support for non-HDLC mode
ChunkaiDeng Nov 21, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
32 changes: 32 additions & 0 deletions router/app_cmds.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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);
Expand All @@ -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);
}
71 changes: 71 additions & 0 deletions router/diag.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,77 @@

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);
}

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)
{
Expand Down
12 changes: 12 additions & 0 deletions router/diag.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@

#include <stdint.h>
#include <sys/types.h>
#include <sys/uio.h>

#include "circ_buf.h"
#include "hdlc.h"
Expand Down Expand Up @@ -64,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 {
Expand Down Expand Up @@ -109,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);
Expand All @@ -125,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,
Expand Down
160 changes: 129 additions & 31 deletions router/dm.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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;

Expand All @@ -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)
Expand All @@ -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)
Expand Down Expand Up @@ -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
Expand All @@ -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;
}
Expand All @@ -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);
}
Expand Down Expand Up @@ -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;
}
}
Loading