Skip to content

Commit

Permalink
DiagID Control packet Handling-1/2
Browse files Browse the repository at this point in the history 
Adding support for Diag-ID handshake and storing the information

Signed-off-by: KyleDengChunkai <[email protected]>
  • Loading branch information
@KyleDengChunkai
KyleDengChunkai committed Nov 25, 2024
1 parent d06e599 commit ca8614f
Show file tree
Hide file tree
Showing 3 changed files with 144 additions and 0 deletions.
2 changes: 2 additions & 0 deletions router/diag.h
View file
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 Expand Up @@ -86,6 +87,7 @@ struct peripheral {
struct watch_flow *flow;

int diag_id;
uint8_t support_diagid_v2;

bool sockets;

Expand Down
125 changes: 125 additions & 0 deletions router/diag_cntl.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -205,6 +205,26 @@ struct diag_cntl_cmd_dereg {
} __packed;
#define to_cmd_dereg(h) container_of(h, struct diag_cntl_cmd_dereg, hdr)

struct diag_cntl_cmd_diag_id {
struct diag_cntl_hdr hdr;
uint32_t version;
uint32_t diag_id;
char process_name[MAX_DIAGID_STR_LEN];
} __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;
uint32_t pd_feature_mask;
char process_name[MAX_DIAGID_STR_LEN];
} __packed;
#define to_cmd_diag_id_v2(h) container_of(h, struct diag_cntl_cmd_diag_id_v2, hdr)

struct list_head diag_id_list = LIST_INIT(diag_id_list);

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 +282,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 +309,10 @@ 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");
peripheral->support_diagid_v2 = 1;
}

printf(" (0x%x)\n", mask);

Expand Down Expand Up @@ -556,6 +582,102 @@ void diag_cntl_set_buffering_mode(struct peripheral *perif, int mode)
}
}

struct list_head* diag_get_diagid_list_head(void)
{
return &diag_id_list;
}

int diag_add_diag_id_to_list(uint8_t diag_id, const char *process_name, struct peripheral *perif)
{
struct diag_id_tbl_t *new_diag_id = NULL;

if (!process_name || diag_id == 0)
return -EINVAL;

new_diag_id = malloc(sizeof(struct diag_id_tbl_t));
if (!new_diag_id)
return -ENOMEM;

new_diag_id->diag_id = diag_id;
memset(new_diag_id->process_name, '\0', sizeof(new_diag_id->process_name));
(void)strncpy(new_diag_id->process_name, process_name, sizeof(new_diag_id->process_name) -1);
list_add(&g_diag_id_list, &new_diag_id->node);

return 0;
}

int diag_get_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, &g_diag_id_list, node) {
if (!strcmp(diag_id_item->process_name, name)) {
*diag_id = diag_id_item->diag_id;
return 1;
}
}

return 0;
}

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);
struct diag_cntl_cmd_diag_id rsp = {0};
uint32_t version = 0, feature_len = 0, local_diag_id = 0;
static uint8_t diag_id = DIAG_ID_APPS;
char *process_name = NULL;
int ret;

if (pkt == NULL || len == 0)
return -EINVAL;

version = pkt->version;
if (peripheral->support_diagid_v2 && version >= DIAGID_VERSION_2) {
if (len < (sizeof(struct diag_cntl_cmd_diag_id_v2 ) -
(MAX_DIAGID_STR_LEN - MIN_DIAGID_STR_LEN))) {
return -EINVAL;
}
feature_len = (uint32_t)pkt_v2->feature_len;
process_name = (char *)&pkt_v2->feature_len +
sizeof(feature_len) + feature_len;
} else {
if (len < (sizeof(struct diag_cntl_cmd_diag_id) -
(MAX_DIAGID_STR_LEN - MIN_DIAGID_STR_LEN))) {
return -EINVAL;
}
process_name = (char*)&pkt->process_name;
fprintf(stderr, "dckdck diag_cntl_process_diag_id get process_name3:%s\n", process_name);
}

ret = diag_get_diag_id(process_name, &local_diag_id);
if (!ret) {
if (version == DIAGID_VERSION_3)
diag_id = pkt_v2->diag_id;
else
diag_id++;

if (diag_add_diag_id_to_list(diag_id, process_name, peripheral))
return -EINVAL;
local_diag_id = diag_id;
}

rsp.diag_id = local_diag_id;
rsp.hdr.cmd = DIAG_CNTL_CMD_DIAG_ID;
rsp.version = DIAGID_VERSION_1;
memset(rsp.process_name, '\0', sizeof(rsp.process_name));
(void)strncpy(rsp.process_name, process_name, sizeof(rsp.process_name) - 1);
rsp.hdr.len = sizeof(rsp.diag_id) + sizeof(rsp.version) + strlen(process_name) + 1;
len = rsp.hdr.len+ sizeof(rsp.hdr);

queue_push(&peripheral->cntlq, &rsp, 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 +700,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
17 changes: 17 additions & 0 deletions router/diag_cntl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,21 @@
#include "peripheral.h"
#include "masks.h"

#define DIAG_CNTL_CMD_DIAG_ID 33
#define MAX_DIAGID_STR_LEN 30
#define MIN_DIAGID_STR_LEN 5

#define DIAGID_VERSION_1 1
#define DIAGID_VERSION_2 2
#define DIAGID_VERSION_3 3
#define DIAG_ID_APPS 1

struct diag_id_tbl_t {
struct list_head node;
uint8_t diag_id;
char process_name[MAX_DIAGID_STR_LEN];
};

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 +61,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_diagid_list_head(void);

#endif

0 comments on commit ca8614f

Please sign in to comment.