linuxmsm/diag
0
0
Fork 0
mirror of https://github.com/linux-msm/diag.git synced 2026-02-25 13:11:48 -08:00
Code Issues Packages Projects Releases Wiki Activity

diag: router: Add handling of Extended Message Configuration (cmd_id=125)

Browse Source 
Implemented and registered a handler for the Extended Message
Configuration command request.

Signed-off-by: Eyal Ilsar <eilsar@codeaurora.org>
[bjorn: Replace posix_memalign with malloc,
	use return value to respond with error message,
	moved implementation to common_cmds.c,
	add peripheral broadcast helper for message masks,
	reworked GET_MSG_MASK to not modify the incoming buffer]
Signed-off-by: Bjorn Andersson <bjorn.andersson@linaro.org>
This commit is contained in:
Eyal Ilsar
2018-01-08 14:32:06 +02:00
committed by Bjorn Andersson
parent 2955443bcc
commit 266bfc4a4f
5 changed files with 253 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

236
common_cmds.c
View File

@@ -55,6 +55,16 @@ struct diag_log_cmd_mask {
#define DIAG_CMD_OP_SET_LOG_MASK 3
#define DIAG_CMD_OP_GET_LOG_MASK 4
#define DIAG_CMD_EXTENDED_MESSAGE_CONFIGURATION 0x7d
#define DIAG_CMD_OP_GET_SSID_RANGE 1
#define DIAG_CMD_OP_GET_BUILD_MASK 2
#define DIAG_CMD_OP_GET_MSG_MASK 3
#define DIAG_CMD_OP_SET_MSG_MASK 4
#define DIAG_CMD_OP_SET_ALL_MSG_MASK 5
#define DIAG_CMD_MSG_STATUS_UNSUCCESSFUL 0
#define DIAG_CMD_MSG_STATUS_SUCCESSFUL 1
static int handle_logging_configuration(struct diag_client *client,
const void *buf, size_t len)
{
@@ -188,7 +198,233 @@ static int handle_logging_configuration(struct diag_client *client,
return 0;
}
static int handle_extended_message_configuration(struct diag_client *client,
const void *buf, size_t len)
{
const struct diag_msg_cmd_header {
uint8_t cmd_code;
uint8_t operation;
}__packed *request_header = buf;
switch (request_header->operation) {
case DIAG_CMD_OP_GET_SSID_RANGE: {
struct {
struct diag_msg_cmd_header header;
uint8_t status;
uint8_t reserved;
uint32_t range_cnt;
struct diag_ssid_range_t ranges[];
} __packed *resp;
uint32_t resp_size = sizeof(*resp);
uint32_t count = 0;
struct diag_ssid_range_t *ranges = NULL;
uint32_t ranges_size = 0;
if (sizeof(*request_header) != len)
return -EMSGSIZE;
diag_cmd_get_ssid_range(&count, &ranges);
ranges_size = count * sizeof(*ranges);
resp_size += ranges_size;
resp = malloc(resp_size);
if (!resp) {
warn("Failed to allocate response packet\n");
return -errno;
}
memcpy(resp, request_header, sizeof(*request_header));
resp->range_cnt = count;
if (ranges != NULL) {
memcpy(resp->ranges, ranges, ranges_size);
free(ranges);
}
resp->status = DIAG_CMD_MSG_STATUS_SUCCESSFUL;
hdlc_enqueue(&client->outq, resp, resp_size);
free(resp);
break;
}
case DIAG_CMD_OP_GET_BUILD_MASK: {
struct diag_ssid_range_t *range = (struct diag_ssid_range_t *)(buf + sizeof(struct diag_msg_cmd_header));
struct {
struct diag_msg_cmd_header header;
uint8_t status;
uint8_t reserved;
uint32_t bld_masks[];
} __packed *resp;
uint32_t resp_size = sizeof(*resp);
uint32_t *masks = NULL;
uint32_t masks_size = 0;
if (sizeof(*request_header) + sizeof(*range) != len)
return -EMSGSIZE;
if (diag_cmd_get_build_mask(range, &masks) == 0) {
masks_size = MSG_RANGE_TO_SIZE(*range);
resp_size += masks_size;
resp = malloc(resp_size);
if (!resp) {
warn("Failed to allocate response packet\n");
return -errno;
}
memcpy(resp, request_header, sizeof(*request_header));
if (masks != NULL) {
memcpy(resp->bld_masks, masks, masks_size);
free(masks);
}
resp->status = DIAG_CMD_MSG_STATUS_SUCCESSFUL;
} else {
resp = malloc(resp_size);
if (!resp) {
warn("Failed to allocate response packet\n");
return -errno;
}
memcpy(resp, request_header, sizeof(*request_header));
resp->status = DIAG_CMD_MSG_STATUS_UNSUCCESSFUL;
}
hdlc_enqueue(&client->outq, resp, resp_size);
free(resp);
break;
}
case DIAG_CMD_OP_GET_MSG_MASK: {
struct diag_ssid_range_t range;
struct {
struct diag_msg_cmd_header header;
uint8_t status;
uint8_t rsvd;
uint32_t rt_masks[];
} __packed *resp;
uint32_t resp_size = sizeof(*resp);
uint32_t *masks = NULL;
uint32_t masks_size = 0;
if (sizeof(*request_header) + sizeof(range) != len)
return -EMSGSIZE;
memcpy(&range, buf + sizeof(struct diag_msg_cmd_header), sizeof(range));
if (diag_cmd_get_msg_mask(&range, &masks) == 0) {
masks_size = MSG_RANGE_TO_SIZE(range);
resp_size += masks_size;
resp = malloc(resp_size);
if (!resp) {
warn("Failed to allocate response packet\n");
return -errno;
}
memcpy(resp, request_header, sizeof(*request_header));
if (masks != NULL) {
memcpy(resp->rt_masks, masks, masks_size);
free(masks);
}
resp->status = DIAG_CMD_MSG_STATUS_SUCCESSFUL;
} else {
resp = malloc(resp_size);
if (!resp) {
warn("Failed to allocate response packet\n");
return -errno;
}
memcpy(resp, request_header, sizeof(*request_header));
resp->status = DIAG_CMD_MSG_STATUS_UNSUCCESSFUL;
}
hdlc_enqueue(&client->outq, resp, resp_size);
free(resp);
break;
}
case DIAG_CMD_OP_SET_MSG_MASK: {
const struct {
struct diag_msg_cmd_header header;
struct diag_ssid_range_t range;
uint16_t rsvd;
uint32_t masks[];
} __packed *req = buf;
struct {
struct diag_msg_cmd_header header;
struct diag_ssid_range_t range;
uint8_t status;
uint8_t rsvd;
uint32_t rt_masks[0];
} __packed *resp;
uint32_t resp_size = sizeof(*resp);
uint32_t masks_size = MSG_RANGE_TO_SIZE(req->range);
if (sizeof(*req) + masks_size != len)
return -EMSGSIZE;
if (diag_cmd_set_msg_mask(req->range, req->masks) == 0) {
resp_size += masks_size;
resp = malloc(resp_size);
if (!resp) {
warn("Failed to allocate response packet\n");
return -errno;
}
resp->header = req->header;
resp->range = req->range;
resp->rsvd = req->rsvd;
if (req->masks != NULL) {
memcpy(resp->rt_masks, req->masks, masks_size);
}
resp->status = DIAG_CMD_MSG_STATUS_SUCCESSFUL;
peripheral_broadcast_msg_mask(&resp->range);
} else {
resp = malloc(resp_size);
if (!resp) {
warn("Failed to allocate response packet\n");
return -errno;
}
resp->header = req->header;
resp->range = req->range;
resp->rsvd = req->rsvd;
resp->status = DIAG_CMD_MSG_STATUS_UNSUCCESSFUL;
}
hdlc_enqueue(&client->outq, resp, resp_size);
free(resp);
break;
}
case DIAG_CMD_OP_SET_ALL_MSG_MASK: {
const struct {
struct diag_msg_cmd_header header;
uint8_t rsvd;
uint32_t mask;
} __packed *req = buf;
struct {
struct diag_msg_cmd_header header;
uint8_t status;
uint8_t rsvd;
uint32_t rt_mask;
} __packed resp;
if (sizeof(*req) != len)
return -EMSGSIZE;
diag_cmd_set_all_msg_mask(req->mask);
resp.header = req->header;
resp.rsvd = req->rsvd;
resp.rt_mask = req->mask;
resp.status = DIAG_CMD_MSG_STATUS_SUCCESSFUL;
peripheral_broadcast_msg_mask(NULL);
hdlc_enqueue(&client->outq, &resp, sizeof(resp));
break;
}
default:
warnx("Unknown extended message configuration: %d", request_header->operation);
return -EINVAL;
}
return 0;
}
void register_common_cmds(void)
{
register_common_cmd(DIAG_CMD_LOGGING_CONFIGURATION, handle_logging_configuration);
register_common_cmd(DIAG_CMD_EXTENDED_MESSAGE_CONFIGURATION, handle_extended_message_configuration);
}

2
masks.c
View File

@@ -461,7 +461,7 @@ int diag_cmd_get_msg_mask(struct diag_ssid_range_t *range, uint32_t **mask)
return 1;
}
int diag_cmd_set_msg_mask(struct diag_ssid_range_t range, uint32_t *mask)
int diag_cmd_set_msg_mask(struct diag_ssid_range_t range, const uint32_t *mask)
{
struct diag_msg_mask_t *msg_item = msg_mask.ptr;
uint32_t num_msgs = 0;

2
masks.h
View File

@@ -138,7 +138,7 @@ int diag_cmd_get_build_mask(struct diag_ssid_range_t *range, uint32_t **mask);
uint8_t diag_get_msg_mask_status();
int diag_cmd_get_msg_mask(struct diag_ssid_range_t *range, uint32_t **mask);
int diag_cmd_set_msg_mask(struct diag_ssid_range_t range, uint32_t *mask);
int diag_cmd_set_msg_mask(struct diag_ssid_range_t range, const uint32_t *mask);
void diag_cmd_set_all_msg_mask(uint32_t mask);
uint8_t diag_get_event_mask_status();

12
peripheral.c
View File

@@ -337,3 +337,15 @@ void peripheral_broadcast_log_mask(unsigned int equip_id)
diag_cntl_send_log_mask(peripheral, equip_id);
}
}
void peripheral_broadcast_msg_mask(struct diag_ssid_range_t *range)
{
struct peripheral *peripheral;
struct list_head *item;
list_for_each(item, &peripherals) {
peripheral = container_of(item, struct peripheral, node);
diag_cntl_send_msg_mask(peripheral, range);
}
}

3
peripheral.h
View File

@@ -31,9 +31,12 @@
#ifndef __PERIPHERAL_H__
#define __PERIPHERAL_H__
struct diag_ssid_range_t;
int peripheral_init(void);
void peripheral_close(struct peripheral *peripheral);
void peripheral_broadcast_log_mask(unsigned int equip_id);
void peripheral_broadcast_msg_mask(struct diag_ssid_range_t *range);
#endif