Skip to content
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

DiagID Control packet Handling #11

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

DiagID Control packet Handling #11

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
e62f312
diag_cntl: Add support for diag id v2 and v3 control commands
KyleDengChunkai Nov 19, 2024
3b3bea7
apps_cmds: Add support for get diag ID command
KyleDengChunkai Nov 25, 2024
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
49 changes: 49 additions & 0 deletions router/app_cmds.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@
#include "dm.h"
#include "hdlc.h"
#include "util.h"
#include "diag_cntl.h"
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think these headers are in alphabetical order, move this under "diag.h"


#define DIAG_CMD_KEEP_ALIVE_SUBSYS 50
#define DIAG_CMD_KEEP_ALIVE_CMD 3
Expand All @@ -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)
{
Expand Down Expand Up @@ -117,11 +121,56 @@ 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_cmd_diag_id_query_req_t {
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

diag_id_query_req

uint8_t cmd_code;
uint8_t subsys_id;
uint16_t subsys_cmd_code;
uint8_t version;
} __packed;
struct diag_cmd_diag_id_query_rsp_t {
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

diag_id_query_resp

struct diag_cmd_diag_id_query_req_t 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 = NULL;
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

offset_resp does not need to be null initialized, the first use of it is to assign it to resp_buffer

uint8_t process_name_len = 0;
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

isn't used anymore?

size_t resp_len = 0;
int num_entries = 0;
KyleDengChunkai marked this conversation as resolved.
Show resolved Hide resolved

if (!buf || len < sizeof(struct diag_cmd_diag_id_query_req_t))
return -EMSGSIZE;

struct diag_cmd_diag_id_query_req_t *req = (struct diag_cmd_diag_id_query_req_t *)buf;
struct diag_cmd_diag_id_query_rsp_t *resp = (struct diag_cmd_diag_id_query_rsp_t *)resp_buffer;
memcpy(resp_buffer, req, sizeof(struct diag_cmd_diag_id_query_req_t));
offset_resp = (uint8_t *)resp_buffer;
resp_len = offsetof(struct diag_cmd_diag_id_query_rsp_t, 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++;
}
KyleDengChunkai marked this conversation as resolved.
Show resolved Hide resolved
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);
register_fallback_cmd(DIAG_CMD_DIAG_VERSION_NO, handle_diag_version_no);
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);
}
1 change: 1 addition & 0 deletions router/diag.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
135 changes: 135 additions & 0 deletions router/diag_cntl.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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);

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

Expand Down Expand Up @@ -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;
KyleDengChunkai marked this conversation as resolved.
Show resolved Hide resolved
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;
Expand All @@ -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:
Expand Down
23 changes: 23 additions & 0 deletions router/diag_cntl.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
KyleDengChunkai marked this conversation as resolved.
Show resolved Hide resolved

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