From 37ff687eda2ec34ec3d7b346c0342873cf42bd97 Mon Sep 17 00:00:00 2001 From: Bjorn Andersson Date: Fri, 6 Jul 2018 12:40:08 -0700 Subject: [PATCH] diag_cntl: Negotiate features Instead of blindly sending a feature mask back to the remote we keep the bits of the remote's mask that we support and send the result back. Signed-off-by: Bjorn Andersson --- diag_cntl.c | 19 +++++++++++-------- diag_cntl.h | 1 - 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/diag_cntl.c b/diag_cntl.c index 980cdf3..c57f95b 100644 --- a/diag_cntl.c +++ b/diag_cntl.c @@ -153,6 +153,8 @@ struct diag_cntl_cmd_dereg { } __packed; #define to_cmd_dereg(h) container_of(h, struct diag_cntl_cmd_dereg, hdr) +static void diag_cntl_send_feature_mask(struct peripheral *peripheral, uint32_t mask); + static int diag_cntl_register(struct peripheral *peripheral, struct diag_cntl_hdr *hdr, size_t len) { @@ -198,8 +200,14 @@ static int diag_cntl_feature_mask(struct peripheral *peripheral, struct diag_cntl_hdr *hdr, size_t len) { struct diag_cntl_cmd_feature *pkt = to_cmd_feature(hdr); + uint32_t local_mask = 0; uint32_t mask = pkt->mask; + local_mask |= DIAG_FEATURE_FEATURE_MASK_SUPPORT; + local_mask |= DIAG_FEATURE_DIAG_MASTER_SETS_COMMON_MASK; + local_mask |= DIAG_FEATURE_REQ_RSP_SUPPORT; + local_mask |= DIAG_FEATURE_APPS_HDLC_ENCODE; + printf("[%s] mask:", peripheral->name); if (mask & DIAG_FEATURE_FEATURE_MASK_SUPPORT) @@ -225,9 +233,9 @@ static int diag_cntl_feature_mask(struct peripheral *peripheral, printf(" (0x%x)\n", mask); - peripheral->features = mask; + peripheral->features = mask & local_mask; - diag_cntl_send_feature_mask(peripheral); + diag_cntl_send_feature_mask(peripheral, peripheral->features); return 0; } @@ -405,21 +413,16 @@ static int diag_cntl_deregister(struct peripheral *peripheral, return 0; } -void diag_cntl_send_feature_mask(struct peripheral *peripheral) +static void diag_cntl_send_feature_mask(struct peripheral *peripheral, uint32_t mask) { struct diag_cntl_cmd_feature *pkt; size_t len = sizeof(*pkt) + 2; - uint32_t mask = 0; if (peripheral->cntl_fd == -1) { warn("Peripheral %s has no control channel. Skipping!\n", peripheral->name); return; } - mask = DIAG_FEATURE_FEATURE_MASK_SUPPORT | - DIAG_FEATURE_DIAG_MASTER_SETS_COMMON_MASK | - DIAG_FEATURE_APPS_HDLC_ENCODE ; - pkt = alloca(len); pkt->hdr.cmd = DIAG_CNTL_CMD_FEATURE_MASK; diff --git a/diag_cntl.h b/diag_cntl.h index 41eb520..d7bf694 100644 --- a/diag_cntl.h +++ b/diag_cntl.h @@ -36,7 +36,6 @@ #include "masks.h" int diag_cntl_recv(struct peripheral *perif, const void *buf, size_t len); -void diag_cntl_send_feature_mask(struct peripheral *peripheral); 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); void diag_cntl_send_event_mask(struct peripheral *peripheral);