diff options
| -rw-r--r-- | drivers/char/diag/diag_dci.c | 51 |
1 files changed, 42 insertions, 9 deletions
diff --git a/drivers/char/diag/diag_dci.c b/drivers/char/diag/diag_dci.c index 2c7662a24cd1..d908e0a5885e 100644 --- a/drivers/char/diag/diag_dci.c +++ b/drivers/char/diag/diag_dci.c @@ -133,6 +133,35 @@ void diag_dci_record_traffic(int read_bytes, uint8_t ch_type, void diag_dci_record_traffic(int read_bytes, uint8_t ch_type, uint8_t peripheral, uint8_t proc) { } #endif + +static int check_peripheral_dci_support(int peripheral_id, int dci_proc_id) +{ + int dci_peripheral_list = 0; + + if (dci_proc_id < 0 || dci_proc_id >= NUM_DCI_PROC) { + pr_err("diag:In %s,not a supported DCI proc id\n", __func__); + return 0; + } + if (peripheral_id < 0 || peripheral_id >= NUM_PERIPHERALS) { + pr_err("diag:In %s,not a valid peripheral id\n", __func__); + return 0; + } + dci_peripheral_list = dci_ops_tbl[dci_proc_id].peripheral_status; + + if (dci_peripheral_list <= 0 || dci_peripheral_list > DIAG_CON_ALL) { + pr_err("diag:In %s,not a valid dci peripheral mask\n", + __func__); + return 0; + } + /* Remove APSS bit mask information */ + dci_peripheral_list = dci_peripheral_list >> 1; + + if ((1 << peripheral_id) & (dci_peripheral_list)) + return 1; + else + return 0; +} + static void create_dci_log_mask_tbl(unsigned char *mask, uint8_t dirty) { unsigned char *temp = mask; @@ -2372,10 +2401,12 @@ int diag_send_dci_event_mask(int token) * is down. It may also mean that the peripheral doesn't * support DCI. */ - err = diag_dci_write_proc(i, DIAG_CNTL_TYPE, buf, - header_size + DCI_EVENT_MASK_SIZE); - if (err != DIAG_DCI_NO_ERROR) - ret = DIAG_DCI_SEND_DATA_FAIL; + if (check_peripheral_dci_support(i, DCI_LOCAL_PROC)) { + err = diag_dci_write_proc(i, DIAG_CNTL_TYPE, buf, + header_size + DCI_EVENT_MASK_SIZE); + if (err != DIAG_DCI_NO_ERROR) + ret = DIAG_DCI_SEND_DATA_FAIL; + } } mutex_unlock(&event_mask.lock); @@ -2557,11 +2588,13 @@ int diag_send_dci_log_mask(int token) } write_len = dci_fill_log_mask(buf, log_mask_ptr); for (j = 0; j < NUM_PERIPHERALS && write_len; j++) { - err = diag_dci_write_proc(j, DIAG_CNTL_TYPE, buf, - write_len); - if (err != DIAG_DCI_NO_ERROR) { - updated = 0; - ret = DIAG_DCI_SEND_DATA_FAIL; + if (check_peripheral_dci_support(j, DCI_LOCAL_PROC)) { + err = diag_dci_write_proc(j, DIAG_CNTL_TYPE, + buf, write_len); + if (err != DIAG_DCI_NO_ERROR) { + updated = 0; + ret = DIAG_DCI_SEND_DATA_FAIL; + } } } if (updated) |