From e62f312abf792187ae235e50319d4bc1ad764b01 Mon Sep 17 00:00:00 2001 From: KyleDengChunkai Date: Tue, 19 Nov 2024 18:57:29 +0800 Subject: [PATCH 1/2] diag_cntl: Add support for diag id v2 and v3 control commands Adding support for Diag-ID handshake and storing the information. Diag-ID indicates and indexes the identity of the clients. Signed-off-by: Kyle Deng --- router/diag.h | 1 + router/diag_cntl.c | 135 +++++++++++++++++++++++++++++++++++++++++++++ router/diag_cntl.h | 23 ++++++++ 3 files changed, 159 insertions(+) diff --git a/router/diag.h b/router/diag.h index da502bf..19270bf 100644 --- a/router/diag.h +++ b/router/diag.h @@ -59,6 +59,7 @@ #define DIAG_FEATURE_DCI_EXTENDED_HEADER BIT(14) #define DIAG_FEATURE_DIAG_ID BIT(15) #define DIAG_FEATURE_PKT_HEADER_UNTAG BIT(16) +#define DIAG_FEATURE_DIAG_ID_FEATURE_MASK BIT(19) #define DIAG_CMD_SUBSYS_DISPATCH 75 #define DIAG_CMD_SUBSYS_DISPATCH_V2 128 diff --git a/router/diag_cntl.c b/router/diag_cntl.c index 0e35c8d..7c94ac9 100644 --- a/router/diag_cntl.c +++ b/router/diag_cntl.c @@ -205,6 +205,27 @@ struct diag_cntl_cmd_dereg { } __packed; #define to_cmd_dereg(h) container_of(h, struct diag_cntl_cmd_dereg, hdr) +#define DIAG_CNTL_CMD_DIAG_ID 33 +struct diag_cntl_cmd_diag_id { + struct diag_cntl_hdr hdr; + uint32_t version; + uint32_t diag_id; + char process_name[]; +} __packed; +#define to_cmd_diagid(h) container_of(h, struct diag_cntl_cmd_diag_id, hdr) + +struct diag_cntl_cmd_diag_id_v2 { + struct diag_cntl_hdr hdr; + uint32_t version; + uint32_t diag_id; + uint32_t feature_len; + uint8_t *feature_mask; + char process_name[]; +} __packed; + #define to_cmd_diag_id_v2(h) container_of(h, struct diag_cntl_cmd_diag_id_v2, hdr) + +struct list_head diag_ids = LIST_INIT(diag_ids); + static void diag_cntl_send_feature_mask(struct peripheral *peripheral, uint32_t mask); static int diag_cntl_register(struct peripheral *peripheral, @@ -262,6 +283,8 @@ static int diag_cntl_feature_mask(struct peripheral *peripheral, local_mask |= DIAG_FEATURE_APPS_HDLC_ENCODE; if (peripheral->sockets) local_mask |= DIAG_FEATURE_SOCKETS_ENABLED; + local_mask |= DIAG_FEATURE_DIAG_ID; + local_mask |= DIAG_FEATURE_DIAG_ID_FEATURE_MASK; printf("[%s] mask:", peripheral->name); @@ -287,6 +310,9 @@ static int diag_cntl_feature_mask(struct peripheral *peripheral, printf(" SOCKETS"); if (mask & DIAG_FEATURE_DIAG_ID) printf(" DIAG-ID"); + if (mask & DIAG_FEATURE_DIAG_ID_FEATURE_MASK) { + printf(" DIAG-ID-FEATURE-MASK"); + } printf(" (0x%x)\n", mask); @@ -556,6 +582,112 @@ void diag_cntl_set_buffering_mode(struct peripheral *perif, int mode) } } +struct list_head *diag_get_diag_ids_head(void) +{ + return &diag_ids; +} + +int register_diag_id(uint8_t diag_id, const char *process_name, uint8_t len) +{ + struct diag_id_tbl_t *new_diag_id = NULL; + + if (!process_name || !len || !diag_id) + return -EINVAL; + + new_diag_id = malloc(sizeof(struct diag_id_tbl_t) + len); + if (!new_diag_id) + return -ENOMEM; + + new_diag_id->diag_id_info_len = sizeof(struct diag_id_info) + len; + new_diag_id->diagid_info.diag_id = diag_id; + new_diag_id->diagid_info.process_name_len = len; + strncpy(new_diag_id->diagid_info.process_name, process_name, len - 1); + new_diag_id->diagid_info.process_name[len - 1] = '\0'; + list_add(&diag_ids, &new_diag_id->node); + + return 0; +} + +int find_diag_id(const char *name, uint32_t *diag_id) +{ + struct diag_id_tbl_t *diag_id_item = NULL; + + if (!name || !diag_id) + return -EINVAL; + + list_for_each_entry(diag_id_item, &diag_ids, node) { + if (!strcmp(diag_id_item->diagid_info.process_name, name)) { + *diag_id = diag_id_item->diagid_info.diag_id; + return 1; + } + } + + return 0; +} + +bool diag_id_exists(uint32_t diag_id) +{ + struct diag_id_tbl_t *diag_id_item = NULL; + + list_for_each_entry(diag_id_item, &diag_ids, node) { + if (diag_id == diag_id_item->diagid_info.diag_id) + return true; + } + + return false; +} + +static int diag_cntl_process_diag_id(struct peripheral *peripheral, struct diag_cntl_hdr *hdr, size_t len) +{ + struct diag_cntl_cmd_diag_id_v2 *pkt_v2 = to_cmd_diag_id_v2(hdr); + struct diag_cntl_cmd_diag_id *pkt = to_cmd_diagid(hdr); + uint32_t version = 0, local_diag_id = 0; + static uint32_t diag_id = DIAG_ID_APPS; + uint8_t resp_buffer[DIAG_MAX_RSP_SIZE] = {0}; + uint8_t process_name_len = 0; + char *process_name = NULL; + int ret; + + version = pkt->version; + if ((peripheral->features & DIAG_FEATURE_DIAG_ID_FEATURE_MASK) && (version >= DIAG_ID_VERSION_2)) { + if (len < sizeof(struct diag_cntl_cmd_diag_id_v2)) + return -EINVAL; + process_name = (char *)&pkt_v2->feature_mask + pkt_v2->feature_len; + } else { + if (len < sizeof(struct diag_cntl_cmd_diag_id)) + return -EINVAL; + process_name = (char*)&pkt->process_name; + } + + process_name_len = strlen(process_name) + 1; + ret = find_diag_id(process_name, &local_diag_id); + if (!ret) { + if (version >= DIAG_ID_VERSION_3) + local_diag_id = pkt_v2->diag_id; + else + local_diag_id = ++diag_id; + + if (diag_id_exists(local_diag_id)) + return -EINVAL; + + if (register_diag_id(local_diag_id, process_name, process_name_len)) + return -EINVAL; + } + diag_id = local_diag_id; + + struct diag_cntl_cmd_diag_id *resp = (struct diag_cntl_cmd_diag_id *)resp_buffer; + resp->diag_id = diag_id; + resp->hdr.cmd = DIAG_CNTL_CMD_DIAG_ID; + resp->version = DIAG_ID_VERSION_1; + strncpy(resp->process_name, process_name, process_name_len - 1); + resp->process_name[process_name_len - 1] = '\0'; + resp->hdr.len = sizeof(resp->diag_id) + sizeof(resp->version) + process_name_len; + len = resp->hdr.len + sizeof(resp->hdr); + + queue_push(&peripheral->cntlq, resp, len); + return 0; +} + int diag_cntl_recv(struct peripheral *peripheral, const void *buf, size_t n) { struct diag_cntl_hdr *hdr; @@ -578,6 +710,9 @@ int diag_cntl_recv(struct peripheral *peripheral, const void *buf, size_t n) case DIAG_CNTL_CMD_FEATURE_MASK: diag_cntl_feature_mask(peripheral, hdr, n); break; + case DIAG_CNTL_CMD_DIAG_ID: + diag_cntl_process_diag_id(peripheral, hdr, n); + break; case DIAG_CNTL_CMD_NUM_PRESETS: break; case DIAG_CNTL_CMD_DEREGISTER: diff --git a/router/diag_cntl.h b/router/diag_cntl.h index f73f0d2..0a9af54 100644 --- a/router/diag_cntl.h +++ b/router/diag_cntl.h @@ -35,6 +35,27 @@ #include "peripheral.h" #include "masks.h" +#define DIAG_ID_VERSION_1 1 +#define DIAG_ID_VERSION_2 2 +#define DIAG_ID_VERSION_3 3 +#define DIAG_ID_APPS 1 + +#define DIAG_MAX_REQ_SIZE (16 * 1024) +#define DIAG_MAX_RSP_SIZE (16 * 1024) + +struct diag_id_info +{ + uint8_t diag_id; + uint8_t process_name_len; + char process_name[]; +}; + +struct diag_id_tbl_t { + struct list_head node; + uint8_t diag_id_info_len; + struct diag_id_info diagid_info; +}; + int diag_cntl_recv(struct peripheral *perif, const void *buf, size_t len); void diag_cntl_send_log_mask(struct peripheral *peripheral, uint32_t equip_id); void diag_cntl_send_msg_mask(struct peripheral *peripheral, struct diag_ssid_range_t *range); @@ -46,4 +67,6 @@ void diag_cntl_send_masks(struct peripheral *peripheral); void diag_cntl_set_diag_mode(struct peripheral *perif, bool real_time); void diag_cntl_set_buffering_mode(struct peripheral *perif, int mode); +struct list_head *diag_get_diag_ids_head(void); + #endif From 4cda4323345dd2ec6f2b382eb4c07a17b88ab524 Mon Sep 17 00:00:00 2001 From: KyleDengChunkai Date: Mon, 25 Nov 2024 17:26:06 +0800 Subject: [PATCH 2/2] apps_cmds: Add support for get diag ID command Adding support for the diag-id command response Signed-off-by: Kyle Deng --- router/app_cmds.c | 48 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/router/app_cmds.c b/router/app_cmds.c index 8382797..cf42a86 100644 --- a/router/app_cmds.c +++ b/router/app_cmds.c @@ -35,6 +35,7 @@ #include #include "diag.h" +#include "diag_cntl.h" #include "dm.h" #include "hdlc.h" #include "util.h" @@ -52,6 +53,9 @@ #define MOBILE_MODEL_STRING "DB410C" #define MSM_REVISION_NUMBER 2 +#define DIAG_CMD_DIAG_SUBSYS 18 +#define DIAG_CMD_DIAG_GET_DIAG_ID 0x222 + static int handle_diag_version(struct diag_client *client, const void *buf, size_t len) { @@ -117,6 +121,48 @@ static int handle_keep_alive(struct diag_client *client, const void *buf, return dm_send(client, resp, sizeof(resp)); } +static int handle_diag_id(struct diag_client *client, const void *buf, size_t len) +{ + struct diag_id_query_req { + uint8_t cmd_code; + uint8_t subsys_id; + uint16_t subsys_cmd_code; + uint8_t version; + } __packed; + struct diag_id_query_resp { + struct diag_id_query_req req_info; + uint8_t num_entries; + uint8_t payload[]; + } __packed; + struct diag_id_tbl_t *diag_id_item = NULL; + struct list_head *diag_ids_head = NULL; + uint8_t resp_buffer[DIAG_MAX_RSP_SIZE] = {0}; + uint8_t *offset_resp; + size_t resp_len = 0; + int num_entries = 0; + + if (!buf || len < sizeof(struct diag_id_query_req)) + return -EMSGSIZE; + + struct diag_id_query_req *req = (struct diag_id_query_req *)buf; + struct diag_id_query_resp *resp = (struct diag_id_query_resp *)resp_buffer; + memcpy(resp_buffer, req, sizeof(struct diag_id_query_req)); + offset_resp = (uint8_t *)resp_buffer; + resp_len = offsetof(struct diag_id_query_resp, payload); + + diag_ids_head = diag_get_diag_ids_head(); + list_for_each_entry(diag_id_item, diag_ids_head, node) { + if (resp_len >= DIAG_MAX_RSP_SIZE) + break; + memcpy(offset_resp + resp_len, &diag_id_item->diagid_info, diag_id_item->diag_id_info_len); + resp_len += diag_id_item->diag_id_info_len; + num_entries++; + } + resp->num_entries = num_entries; + + return dm_send(client, resp_buffer, resp_len); +} + void register_app_cmds(void) { register_fallback_cmd(DIAG_CMD_DIAG_VERSION_ID, handle_diag_version); @@ -124,4 +170,6 @@ void register_app_cmds(void) register_fallback_cmd(DIAG_CMD_EXTENDED_BUILD_ID, handle_extended_build_id); register_fallback_subsys_cmd(DIAG_CMD_KEEP_ALIVE_SUBSYS, 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); }