From d8c5876fd7b12c328406ff949a821645cda0a5b9 Mon Sep 17 00:00:00 2001 From: Manoj Prabhu B Date: Mon, 24 Apr 2017 15:12:05 +0530 Subject: [PATCH] diag: Add PD buffering mode configuration support This patch extends support to configure buffering modes for PD buffers by defining new control packets. CRs-Fixed: 2019180 Change-Id: I5a72ff3ec7e5ab80a7461c3c3130e3b699e75522 Signed-off-by: Manoj Prabhu B --- drivers/char/diag/diag_debugfs.c | 9 +- drivers/char/diag/diagchar.h | 6 +- drivers/char/diag/diagchar_core.c | 44 +++- drivers/char/diag/diagfwd.c | 5 + drivers/char/diag/diagfwd_cntl.c | 339 ++++++++++++++++++++++-------- drivers/char/diag/diagfwd_cntl.h | 54 ++++- 6 files changed, 352 insertions(+), 105 deletions(-) diff --git a/drivers/char/diag/diag_debugfs.c b/drivers/char/diag/diag_debugfs.c index ca7dd88048ac..62ef9acc1df3 100644 --- a/drivers/char/diag/diag_debugfs.c +++ b/drivers/char/diag/diag_debugfs.c @@ -80,7 +80,8 @@ static ssize_t diag_dbgfs_read_status(struct file *file, char __user *ubuf, "Time Sync Enabled: %d\n" "MD session mode: %d\n" "MD session mask: %d\n" - "Uses Time API: %d\n", + "Uses Time API: %d\n" + "Supports PD buffering: %d\n", chk_config_get_id(), chk_polling_response(), driver->polling_reg_flag, @@ -95,11 +96,12 @@ static ssize_t diag_dbgfs_read_status(struct file *file, char __user *ubuf, driver->time_sync_enabled, driver->md_session_mode, driver->md_session_mask, - driver->uses_time_api); + driver->uses_time_api, + driver->supports_pd_buffering); for (i = 0; i < NUM_PERIPHERALS; i++) { ret += scnprintf(buf+ret, buf_size-ret, - "p: %s Feature: %02x %02x |%c%c%c%c%c%c%c%c%c|\n", + "p: %s Feature: %02x %02x |%c%c%c%c%c%c%c%c%c%c|\n", PERIPHERAL_STRING(i), driver->feature[i].feature_mask[0], driver->feature[i].feature_mask[1], @@ -108,6 +110,7 @@ static ssize_t diag_dbgfs_read_status(struct file *file, char __user *ubuf, driver->feature[i].separate_cmd_rsp ? 'C':'c', driver->feature[i].encode_hdlc ? 'H':'h', driver->feature[i].mask_centralization ? 'M':'m', + driver->feature[i].pd_buffering ? 'P':'p', driver->feature[i].stm_support ? 'Q':'q', driver->feature[i].sockets_enabled ? 'S':'s', driver->feature[i].sent_feature_mask ? 'T':'t', diff --git a/drivers/char/diag/diagchar.h b/drivers/char/diag/diagchar.h index b17538a10ea9..60160997a4e2 100644 --- a/drivers/char/diag/diagchar.h +++ b/drivers/char/diag/diagchar.h @@ -478,6 +478,7 @@ struct diag_feature_t { uint8_t encode_hdlc; uint8_t untag_header; uint8_t peripheral_buffering; + uint8_t pd_buffering; uint8_t mask_centralization; uint8_t stm_support; uint8_t sockets_enabled; @@ -509,6 +510,7 @@ struct diagchar_dev { int supports_separate_cmdrsp; int supports_apps_hdlc_encoding; int supports_apps_header_untagging; + int supports_pd_buffering; int peripheral_untag[NUM_PERIPHERALS]; int supports_sockets; /* The state requested in the STM command */ @@ -561,8 +563,8 @@ struct diagchar_dev { struct diagfwd_info *diagfwd_cmd[NUM_PERIPHERALS]; struct diagfwd_info *diagfwd_dci_cmd[NUM_PERIPHERALS]; struct diag_feature_t feature[NUM_PERIPHERALS]; - struct diag_buffering_mode_t buffering_mode[NUM_PERIPHERALS]; - uint8_t buffering_flag[NUM_PERIPHERALS]; + struct diag_buffering_mode_t buffering_mode[NUM_MD_SESSIONS]; + uint8_t buffering_flag[NUM_MD_SESSIONS]; struct mutex mode_lock; unsigned char *user_space_data_buf; uint8_t user_space_data_busy; diff --git a/drivers/char/diag/diagchar_core.c b/drivers/char/diag/diagchar_core.c index 682c035c5bd4..e4e13ce7dd22 100644 --- a/drivers/char/diag/diagchar_core.c +++ b/drivers/char/diag/diagchar_core.c @@ -1908,12 +1908,33 @@ static int diag_ioctl_get_real_time(unsigned long ioarg) static int diag_ioctl_set_buffering_mode(unsigned long ioarg) { struct diag_buffering_mode_t params; + int peripheral = 0; + uint8_t diag_id = 0; if (copy_from_user(¶ms, (void __user *)ioarg, sizeof(params))) return -EFAULT; - if (params.peripheral >= NUM_PERIPHERALS) - return -EINVAL; + diag_map_pd_to_diagid(params.peripheral, &diag_id, &peripheral); + + if ((peripheral < 0) || + peripheral >= NUM_PERIPHERALS) { + pr_err("diag: In %s, invalid peripheral %d\n", __func__, + peripheral); + return -EIO; + } + + if (params.peripheral > NUM_PERIPHERALS && + !driver->feature[peripheral].pd_buffering) { + pr_err("diag: In %s, pd buffering not supported for peripheral:%d\n", + __func__, peripheral); + return -EIO; + } + + if (!driver->feature[peripheral].peripheral_buffering) { + pr_err("diag: In %s, peripheral %d doesn't support buffering\n", + __func__, peripheral); + return -EIO; + } mutex_lock(&driver->mode_lock); driver->buffering_flag[params.peripheral] = 1; @@ -1924,24 +1945,29 @@ static int diag_ioctl_set_buffering_mode(unsigned long ioarg) static int diag_ioctl_peripheral_drain_immediate(unsigned long ioarg) { - uint8_t peripheral; + uint8_t pd, diag_id = 0; + int peripheral = 0; - if (copy_from_user(&peripheral, (void __user *)ioarg, sizeof(uint8_t))) + if (copy_from_user(&pd, (void __user *)ioarg, sizeof(uint8_t))) return -EFAULT; - if (peripheral >= NUM_PERIPHERALS) { + diag_map_pd_to_diagid(pd, &diag_id, &peripheral); + + if ((peripheral < 0) || + peripheral >= NUM_PERIPHERALS) { pr_err("diag: In %s, invalid peripheral %d\n", __func__, peripheral); return -EINVAL; } - if (!driver->feature[peripheral].peripheral_buffering) { - pr_err("diag: In %s, peripheral %d doesn't support buffering\n", - __func__, peripheral); + if (pd > NUM_PERIPHERALS && + !driver->feature[peripheral].pd_buffering) { + pr_err("diag: In %s, pd buffering not supported for peripheral:%d\n", + __func__, peripheral); return -EIO; } - return diag_send_peripheral_drain_immediate(peripheral); + return diag_send_peripheral_drain_immediate(pd, diag_id, peripheral); } static int diag_ioctl_dci_support(unsigned long ioarg) diff --git a/drivers/char/diag/diagfwd.c b/drivers/char/diag/diagfwd.c index 8fb724305c03..019bf1946ac3 100644 --- a/drivers/char/diag/diagfwd.c +++ b/drivers/char/diag/diagfwd.c @@ -1601,6 +1601,7 @@ int diagfwd_init(void) driver->supports_separate_cmdrsp = 1; driver->supports_apps_hdlc_encoding = 1; driver->supports_apps_header_untagging = 1; + driver->supports_pd_buffering = 1; for (i = 0; i < NUM_PERIPHERALS; i++) driver->peripheral_untag[i] = 0; mutex_init(&driver->diag_hdlc_mutex); @@ -1631,12 +1632,16 @@ int diagfwd_init(void) driver->feature[i].stm_support = DISABLE_STM; driver->feature[i].rcvd_feature_mask = 0; driver->feature[i].peripheral_buffering = 0; + driver->feature[i].pd_buffering = 0; driver->feature[i].encode_hdlc = 0; driver->feature[i].untag_header = DISABLE_PKT_HEADER_UNTAGGING; driver->feature[i].mask_centralization = 0; driver->feature[i].log_on_demand = 0; driver->feature[i].sent_feature_mask = 0; + } + + for (i = 0; i < NUM_MD_SESSIONS; i++) { driver->buffering_mode[i].peripheral = i; driver->buffering_mode[i].mode = DIAG_BUFFERING_MODE_STREAMING; driver->buffering_mode[i].high_wm_val = DEFAULT_HIGH_WM_VAL; diff --git a/drivers/char/diag/diagfwd_cntl.c b/drivers/char/diag/diagfwd_cntl.c index 82a67f1f6f47..605aec88d39c 100644 --- a/drivers/char/diag/diagfwd_cntl.c +++ b/drivers/char/diag/diagfwd_cntl.c @@ -418,6 +418,8 @@ static void process_incoming_feature_mask(uint8_t *buf, uint32_t len, driver->feature[peripheral].mask_centralization = 1; if (FEATURE_SUPPORTED(F_DIAG_PERIPHERAL_BUFFERING)) driver->feature[peripheral].peripheral_buffering = 1; + if (FEATURE_SUPPORTED(F_DIAG_PD_BUFFERING)) + driver->feature[peripheral].pd_buffering = 1; if (FEATURE_SUPPORTED(F_DIAG_SOCKETS_ENABLED)) enable_socket_feature(peripheral); } @@ -789,32 +791,54 @@ static int diag_compute_real_time(int idx) } static void diag_create_diag_mode_ctrl_pkt(unsigned char *dest_buf, - int real_time) + uint8_t diag_id, int real_time) { struct diag_ctrl_msg_diagmode diagmode; + struct diag_ctrl_msg_diagmode_v2 diagmode_v2; int msg_size = sizeof(struct diag_ctrl_msg_diagmode); + int msg_size_2 = sizeof(struct diag_ctrl_msg_diagmode_v2); if (!dest_buf) return; - diagmode.ctrl_pkt_id = DIAG_CTRL_MSG_DIAGMODE; - diagmode.ctrl_pkt_data_len = DIAG_MODE_PKT_LEN; - diagmode.version = 1; - diagmode.sleep_vote = real_time ? 1 : 0; - /* - * 0 - Disables real-time logging (to prevent - * frequent APPS wake-ups, etc.). - * 1 - Enable real-time logging - */ - diagmode.real_time = real_time; - diagmode.use_nrt_values = 0; - diagmode.commit_threshold = 0; - diagmode.sleep_threshold = 0; - diagmode.sleep_time = 0; - diagmode.drain_timer_val = 0; - diagmode.event_stale_timer_val = 0; - - memcpy(dest_buf, &diagmode, msg_size); + if (diag_id) { + diagmode_v2.ctrl_pkt_id = DIAG_CTRL_MSG_DIAGMODE; + diagmode_v2.ctrl_pkt_data_len = DIAG_MODE_PKT_LEN_V2; + diagmode_v2.version = 2; + diagmode_v2.sleep_vote = real_time ? 1 : 0; + /* + * 0 - Disables real-time logging (to prevent + * frequent APPS wake-ups, etc.). + * 1 - Enable real-time logging + */ + diagmode_v2.real_time = real_time; + diagmode_v2.use_nrt_values = 0; + diagmode_v2.commit_threshold = 0; + diagmode_v2.sleep_threshold = 0; + diagmode_v2.sleep_time = 0; + diagmode_v2.drain_timer_val = 0; + diagmode_v2.event_stale_timer_val = 0; + diagmode_v2.diag_id = diag_id; + memcpy(dest_buf, &diagmode_v2, msg_size_2); + } else { + diagmode.ctrl_pkt_id = DIAG_CTRL_MSG_DIAGMODE; + diagmode.ctrl_pkt_data_len = DIAG_MODE_PKT_LEN; + diagmode.version = 1; + diagmode.sleep_vote = real_time ? 1 : 0; + /* + * 0 - Disables real-time logging (to prevent + * frequent APPS wake-ups, etc.). + * 1 - Enable real-time logging + */ + diagmode.real_time = real_time; + diagmode.use_nrt_values = 0; + diagmode.commit_threshold = 0; + diagmode.sleep_threshold = 0; + diagmode.sleep_time = 0; + diagmode.drain_timer_val = 0; + diagmode.event_stale_timer_val = 0; + memcpy(dest_buf, &diagmode, msg_size); + } } void diag_update_proc_vote(uint16_t proc, uint8_t vote, int index) @@ -899,7 +923,7 @@ static void diag_send_diag_mode_update_remote(int token, int real_time) memcpy(buf + write_len, &dci_header, dci_header_size); write_len += dci_header_size; - diag_create_diag_mode_ctrl_pkt(buf + write_len, real_time); + diag_create_diag_mode_ctrl_pkt(buf + write_len, 0, real_time); write_len += msg_size; *(buf + write_len) = CONTROL_CHAR; /* End Terminator */ write_len += sizeof(uint8_t); @@ -1005,14 +1029,18 @@ void diag_real_time_work_fn(struct work_struct *work) } #endif -static int __diag_send_real_time_update(uint8_t peripheral, int real_time) +static int __diag_send_real_time_update(uint8_t peripheral, int real_time, + uint8_t diag_id) { - char buf[sizeof(struct diag_ctrl_msg_diagmode)]; - int msg_size = sizeof(struct diag_ctrl_msg_diagmode); + char buf[sizeof(struct diag_ctrl_msg_diagmode_v2)]; + int msg_size = 0; int err = 0; - if (peripheral >= NUM_PERIPHERALS) + if (peripheral >= NUM_PERIPHERALS) { + pr_err("diag: In %s, invalid peripheral %d\n", __func__, + peripheral); return -EINVAL; + } if (!driver->diagfwd_cntl[peripheral] || !driver->diagfwd_cntl[peripheral]->ch_open) { @@ -1027,12 +1055,17 @@ static int __diag_send_real_time_update(uint8_t peripheral, int real_time) return -EINVAL; } - diag_create_diag_mode_ctrl_pkt(buf, real_time); + msg_size = (diag_id ? sizeof(struct diag_ctrl_msg_diagmode_v2) : + sizeof(struct diag_ctrl_msg_diagmode)); + + diag_create_diag_mode_ctrl_pkt(buf, diag_id, real_time); mutex_lock(&driver->diag_cntl_mutex); + err = diagfwd_write(peripheral, TYPE_CNTL, buf, msg_size); + if (err && err != -ENODEV) { - pr_err("diag: In %s, unable to write to smd, peripheral: %d, type: %d, len: %d, err: %d\n", + pr_err("diag: In %s, unable to write, peripheral: %d, type: %d, len: %d, err: %d\n", __func__, peripheral, TYPE_CNTL, msg_size, err); } else { @@ -1058,27 +1091,79 @@ int diag_send_real_time_update(uint8_t peripheral, int real_time) return -EINVAL; } - return __diag_send_real_time_update(peripheral, real_time); + return __diag_send_real_time_update(peripheral, real_time, 0); +} + +void diag_map_pd_to_diagid(uint8_t pd, uint8_t *diag_id, int *peripheral) +{ + switch (pd) { + case UPD_WLAN: + *diag_id = DIAG_ID_WLAN; + *peripheral = PERIPHERAL_MODEM; + break; + case UPD_AUDIO: + *diag_id = DIAG_ID_AUDIO; + *peripheral = PERIPHERAL_LPASS; + break; + case UPD_SENSORS: + *diag_id = DIAG_ID_SENSORS; + *peripheral = PERIPHERAL_LPASS; + break; + case PERIPHERAL_MODEM: + *diag_id = DIAG_ID_MPSS; + *peripheral = PERIPHERAL_MODEM; + break; + case PERIPHERAL_LPASS: + *diag_id = DIAG_ID_LPASS; + *peripheral = PERIPHERAL_LPASS; + break; + case PERIPHERAL_CDSP: + *diag_id = DIAG_ID_CDSP; + *peripheral = PERIPHERAL_CDSP; + break; + default: + pr_err("diag: In %s, invalid peripheral %d\n", __func__, + pd); + *peripheral = -EINVAL; + break; + } + + if (*peripheral > 0) + if (!driver->feature[*peripheral].pd_buffering) + *diag_id = 0; } int diag_send_peripheral_buffering_mode(struct diag_buffering_mode_t *params) { int err = 0; int mode = MODE_REALTIME; - uint8_t peripheral = 0; + int peripheral = 0; + uint8_t diag_id = 0; if (!params) return -EIO; - peripheral = params->peripheral; - if (peripheral >= NUM_PERIPHERALS) { + diag_map_pd_to_diagid(params->peripheral, + &diag_id, &peripheral); + + if ((peripheral < 0) || + peripheral >= NUM_PERIPHERALS) { pr_err("diag: In %s, invalid peripheral %d\n", __func__, peripheral); return -EINVAL; } - if (!driver->buffering_flag[peripheral]) + if (!driver->buffering_flag[params->peripheral]) { + pr_err("diag: In %s, buffering flag not set for %d\n", __func__, + params->peripheral); return -EINVAL; + } + + if (!driver->feature[peripheral].peripheral_buffering) { + pr_err("diag: In %s, peripheral %d doesn't support buffering\n", + __func__, peripheral); + return -EIO; + } switch (params->mode) { case DIAG_BUFFERING_MODE_STREAMING: @@ -1097,7 +1182,7 @@ int diag_send_peripheral_buffering_mode(struct diag_buffering_mode_t *params) if (!driver->feature[peripheral].peripheral_buffering) { pr_debug("diag: In %s, peripheral %d doesn't support buffering\n", __func__, peripheral); - driver->buffering_flag[peripheral] = 0; + driver->buffering_flag[params->peripheral] = 0; return -EIO; } @@ -1112,35 +1197,39 @@ int diag_send_peripheral_buffering_mode(struct diag_buffering_mode_t *params) (params->low_wm_val != DIAG_MIN_WM_VAL))) { pr_err("diag: In %s, invalid watermark values, high: %d, low: %d, peripheral: %d\n", __func__, params->high_wm_val, params->low_wm_val, - peripheral); + params->peripheral); return -EINVAL; } mutex_lock(&driver->mode_lock); - err = diag_send_buffering_tx_mode_pkt(peripheral, params); + err = diag_send_buffering_tx_mode_pkt(peripheral, diag_id, params); if (err) { pr_err("diag: In %s, unable to send buffering mode packet to peripheral %d, err: %d\n", __func__, peripheral, err); goto fail; } - err = diag_send_buffering_wm_values(peripheral, params); + err = diag_send_buffering_wm_values(peripheral, diag_id, params); if (err) { pr_err("diag: In %s, unable to send buffering wm value packet to peripheral %d, err: %d\n", __func__, peripheral, err); goto fail; } - err = __diag_send_real_time_update(peripheral, mode); + err = __diag_send_real_time_update(peripheral, mode, diag_id); if (err) { pr_err("diag: In %s, unable to send mode update to peripheral %d, mode: %d, err: %d\n", __func__, peripheral, mode, err); goto fail; } - driver->buffering_mode[peripheral].peripheral = peripheral; - driver->buffering_mode[peripheral].mode = params->mode; - driver->buffering_mode[peripheral].low_wm_val = params->low_wm_val; - driver->buffering_mode[peripheral].high_wm_val = params->high_wm_val; + driver->buffering_mode[params->peripheral].peripheral = + params->peripheral; + driver->buffering_mode[params->peripheral].mode = + params->mode; + driver->buffering_mode[params->peripheral].low_wm_val = + params->low_wm_val; + driver->buffering_mode[params->peripheral].high_wm_val = + params->high_wm_val; if (params->mode == DIAG_BUFFERING_MODE_STREAMING) - driver->buffering_flag[peripheral] = 0; + driver->buffering_flag[params->peripheral] = 0; fail: mutex_unlock(&driver->mode_lock); return err; @@ -1179,10 +1268,12 @@ int diag_send_stm_state(uint8_t peripheral, uint8_t stm_control_data) return err; } -int diag_send_peripheral_drain_immediate(uint8_t peripheral) +int diag_send_peripheral_drain_immediate(uint8_t pd, + uint8_t diag_id, int peripheral) { int err = 0; struct diag_ctrl_drain_immediate ctrl_pkt; + struct diag_ctrl_drain_immediate_v2 ctrl_pkt_v2; if (!driver->feature[peripheral].peripheral_buffering) { pr_debug("diag: In %s, peripheral %d doesn't support buffering\n", @@ -1197,32 +1288,57 @@ int diag_send_peripheral_drain_immediate(uint8_t peripheral) return -ENODEV; } - ctrl_pkt.pkt_id = DIAG_CTRL_MSG_PERIPHERAL_BUF_DRAIN_IMM; - /* The length of the ctrl pkt is size of version and stream id */ - ctrl_pkt.len = sizeof(uint32_t) + sizeof(uint8_t); - ctrl_pkt.version = 1; - ctrl_pkt.stream_id = 1; - - err = diagfwd_write(peripheral, TYPE_CNTL, &ctrl_pkt, sizeof(ctrl_pkt)); - if (err && err != -ENODEV) { - pr_err("diag: Unable to send drain immediate ctrl packet to peripheral %d, err: %d\n", - peripheral, err); + if (diag_id && driver->feature[peripheral].pd_buffering) { + ctrl_pkt_v2.pkt_id = DIAG_CTRL_MSG_PERIPHERAL_BUF_DRAIN_IMM; + /* + * The length of the ctrl pkt is size of version, + * diag_id and stream id + */ + ctrl_pkt_v2.len = sizeof(uint32_t) + (2 * sizeof(uint8_t)); + ctrl_pkt_v2.version = 2; + ctrl_pkt_v2.diag_id = diag_id; + ctrl_pkt_v2.stream_id = 1; + err = diagfwd_write(peripheral, TYPE_CNTL, &ctrl_pkt_v2, + sizeof(ctrl_pkt_v2)); + if (err && err != -ENODEV) { + pr_err("diag: Unable to send drain immediate ctrl packet to peripheral %d, err: %d\n", + peripheral, err); + } + } else { + ctrl_pkt.pkt_id = DIAG_CTRL_MSG_PERIPHERAL_BUF_DRAIN_IMM; + /* + * The length of the ctrl pkt is + * size of version and stream id + */ + ctrl_pkt.len = sizeof(uint32_t) + sizeof(uint8_t); + ctrl_pkt.version = 1; + ctrl_pkt.stream_id = 1; + err = diagfwd_write(peripheral, TYPE_CNTL, &ctrl_pkt, + sizeof(ctrl_pkt)); + if (err && err != -ENODEV) { + pr_err("diag: Unable to send drain immediate ctrl packet to peripheral %d, err: %d\n", + peripheral, err); + } } return err; } int diag_send_buffering_tx_mode_pkt(uint8_t peripheral, - struct diag_buffering_mode_t *params) + uint8_t diag_id, struct diag_buffering_mode_t *params) { int err = 0; struct diag_ctrl_peripheral_tx_mode ctrl_pkt; + struct diag_ctrl_peripheral_tx_mode_v2 ctrl_pkt_v2; if (!params) return -EIO; - if (peripheral >= NUM_PERIPHERALS) + if (peripheral >= NUM_PERIPHERALS) { + pr_err("diag: In %s, invalid peripheral %d\n", __func__, + peripheral); return -EINVAL; + } if (!driver->feature[peripheral].peripheral_buffering) { pr_debug("diag: In %s, peripheral %d doesn't support buffering\n", @@ -1230,9 +1346,6 @@ int diag_send_buffering_tx_mode_pkt(uint8_t peripheral, return -EINVAL; } - if (params->peripheral != peripheral) - return -EINVAL; - switch (params->mode) { case DIAG_BUFFERING_MODE_STREAMING: case DIAG_BUFFERING_MODE_THRESHOLD: @@ -1244,36 +1357,67 @@ int diag_send_buffering_tx_mode_pkt(uint8_t peripheral, return -EINVAL; } - ctrl_pkt.pkt_id = DIAG_CTRL_MSG_CONFIG_PERIPHERAL_TX_MODE; - /* Control packet length is size of version, stream_id and tx_mode */ - ctrl_pkt.len = sizeof(uint32_t) + (2 * sizeof(uint8_t)); - ctrl_pkt.version = 1; - ctrl_pkt.stream_id = 1; - ctrl_pkt.tx_mode = params->mode; + if (diag_id && + driver->feature[peripheral].pd_buffering) { - err = diagfwd_write(peripheral, TYPE_CNTL, &ctrl_pkt, sizeof(ctrl_pkt)); - if (err && err != -ENODEV) { - pr_err("diag: Unable to send tx_mode ctrl packet to peripheral %d, err: %d\n", - peripheral, err); - goto fail; + ctrl_pkt_v2.pkt_id = DIAG_CTRL_MSG_CONFIG_PERIPHERAL_TX_MODE; + /* + * Control packet length is size of version, diag_id, + * stream_id and tx_mode + */ + ctrl_pkt_v2.len = sizeof(uint32_t) + (3 * sizeof(uint8_t)); + ctrl_pkt_v2.version = 2; + ctrl_pkt_v2.diag_id = diag_id; + ctrl_pkt_v2.stream_id = 1; + ctrl_pkt_v2.tx_mode = params->mode; + + err = diagfwd_write(peripheral, TYPE_CNTL, &ctrl_pkt_v2, + sizeof(ctrl_pkt_v2)); + if (err && err != -ENODEV) { + pr_err("diag: Unable to send tx_mode ctrl packet to peripheral %d, err: %d\n", + peripheral, err); + goto fail; + } + } else { + ctrl_pkt.pkt_id = DIAG_CTRL_MSG_CONFIG_PERIPHERAL_TX_MODE; + /* + * Control packet length is size of version, + * stream_id and tx_mode + */ + ctrl_pkt.len = sizeof(uint32_t) + (2 * sizeof(uint8_t)); + ctrl_pkt.version = 1; + ctrl_pkt.stream_id = 1; + ctrl_pkt.tx_mode = params->mode; + + err = diagfwd_write(peripheral, TYPE_CNTL, &ctrl_pkt, + sizeof(ctrl_pkt)); + if (err && err != -ENODEV) { + pr_err("diag: Unable to send tx_mode ctrl packet to peripheral %d, err: %d\n", + peripheral, err); + goto fail; + } } - driver->buffering_mode[peripheral].mode = params->mode; + driver->buffering_mode[params->peripheral].mode = params->mode; fail: return err; } int diag_send_buffering_wm_values(uint8_t peripheral, - struct diag_buffering_mode_t *params) + uint8_t diag_id, struct diag_buffering_mode_t *params) { int err = 0; struct diag_ctrl_set_wq_val ctrl_pkt; + struct diag_ctrl_set_wq_val_v2 ctrl_pkt_v2; if (!params) return -EIO; - if (peripheral >= NUM_PERIPHERALS) + if (peripheral >= NUM_PERIPHERALS) { + pr_err("diag: In %s, invalid peripheral %d\n", __func__, + peripheral); return -EINVAL; + } if (!driver->feature[peripheral].peripheral_buffering) { pr_debug("diag: In %s, peripheral %d doesn't support buffering\n", @@ -1288,9 +1432,6 @@ int diag_send_buffering_wm_values(uint8_t peripheral, return -ENODEV; } - if (params->peripheral != peripheral) - return -EINVAL; - switch (params->mode) { case DIAG_BUFFERING_MODE_STREAMING: case DIAG_BUFFERING_MODE_THRESHOLD: @@ -1302,21 +1443,45 @@ int diag_send_buffering_wm_values(uint8_t peripheral, return -EINVAL; } - ctrl_pkt.pkt_id = DIAG_CTRL_MSG_CONFIG_PERIPHERAL_WMQ_VAL; - /* Control packet length is size of version, stream_id and wmq values */ - ctrl_pkt.len = sizeof(uint32_t) + (3 * sizeof(uint8_t)); - ctrl_pkt.version = 1; - ctrl_pkt.stream_id = 1; - ctrl_pkt.high_wm_val = params->high_wm_val; - ctrl_pkt.low_wm_val = params->low_wm_val; + if (diag_id && + driver->feature[peripheral].pd_buffering) { + ctrl_pkt_v2.pkt_id = DIAG_CTRL_MSG_CONFIG_PERIPHERAL_WMQ_VAL; + /* + * Control packet length is size of version, diag_id, + * stream_id and wmq values + */ + ctrl_pkt_v2.len = sizeof(uint32_t) + (4 * sizeof(uint8_t)); + ctrl_pkt_v2.version = 2; + ctrl_pkt_v2.diag_id = diag_id; + ctrl_pkt_v2.stream_id = 1; + ctrl_pkt_v2.high_wm_val = params->high_wm_val; + ctrl_pkt_v2.low_wm_val = params->low_wm_val; - err = diagfwd_write(peripheral, TYPE_CNTL, &ctrl_pkt, - sizeof(ctrl_pkt)); - if (err && err != -ENODEV) { - pr_err("diag: Unable to send watermark values to peripheral %d, err: %d\n", - peripheral, err); + err = diagfwd_write(peripheral, TYPE_CNTL, &ctrl_pkt_v2, + sizeof(ctrl_pkt_v2)); + if (err && err != -ENODEV) { + pr_err("diag: Unable to send watermark values to peripheral %d, err: %d\n", + peripheral, err); + } + } else { + ctrl_pkt.pkt_id = DIAG_CTRL_MSG_CONFIG_PERIPHERAL_WMQ_VAL; + /* + * Control packet length is size of version, + * stream_id and wmq values + */ + ctrl_pkt.len = sizeof(uint32_t) + (3 * sizeof(uint8_t)); + ctrl_pkt.version = 1; + ctrl_pkt.stream_id = 1; + ctrl_pkt.high_wm_val = params->high_wm_val; + ctrl_pkt.low_wm_val = params->low_wm_val; + + err = diagfwd_write(peripheral, TYPE_CNTL, &ctrl_pkt, + sizeof(ctrl_pkt)); + if (err && err != -ENODEV) { + pr_err("diag: Unable to send watermark values to peripheral %d, err: %d\n", + peripheral, err); + } } - return err; } diff --git a/drivers/char/diag/diagfwd_cntl.h b/drivers/char/diag/diagfwd_cntl.h index e8608f47ff14..86442d838471 100644 --- a/drivers/char/diag/diagfwd_cntl.h +++ b/drivers/char/diag/diagfwd_cntl.h @@ -68,6 +68,7 @@ #define F_DIAG_SOCKETS_ENABLED 13 #define F_DIAG_DCI_EXTENDED_HEADER_SUPPORT 14 #define F_DIAG_PKT_HEADER_UNTAG 16 +#define F_DIAG_PD_BUFFERING 17 #define ENABLE_SEPARATE_CMDRSP 1 #define DISABLE_SEPARATE_CMDRSP 0 @@ -85,7 +86,8 @@ #define ENABLE_PKT_HEADER_UNTAGGING 1 #define DISABLE_PKT_HEADER_UNTAGGING 0 -#define DIAG_MODE_PKT_LEN 36 +#define DIAG_MODE_PKT_LEN 36 +#define DIAG_MODE_PKT_LEN_V2 37 struct diag_ctrl_pkt_header_t { uint32_t pkt_id; @@ -171,6 +173,21 @@ struct diag_ctrl_msg_diagmode { uint32_t event_stale_timer_val; } __packed; +struct diag_ctrl_msg_diagmode_v2 { + uint32_t ctrl_pkt_id; + uint32_t ctrl_pkt_data_len; + uint32_t version; + uint32_t sleep_vote; + uint32_t real_time; + uint32_t use_nrt_values; + uint32_t commit_threshold; + uint32_t sleep_threshold; + uint32_t sleep_time; + uint32_t drain_timer_val; + uint32_t event_stale_timer_val; + uint8_t diag_id; +} __packed; + struct diag_ctrl_msg_stm { uint32_t ctrl_pkt_id; uint32_t ctrl_pkt_data_len; @@ -249,6 +266,15 @@ struct diag_ctrl_peripheral_tx_mode { uint8_t tx_mode; } __packed; +struct diag_ctrl_peripheral_tx_mode_v2 { + uint32_t pkt_id; + uint32_t len; + uint32_t version; + uint8_t diag_id; + uint8_t stream_id; + uint8_t tx_mode; +} __packed; + struct diag_ctrl_drain_immediate { uint32_t pkt_id; uint32_t len; @@ -256,6 +282,14 @@ struct diag_ctrl_drain_immediate { uint8_t stream_id; } __packed; +struct diag_ctrl_drain_immediate_v2 { + uint32_t pkt_id; + uint32_t len; + uint32_t version; + uint8_t diag_id; + uint8_t stream_id; +} __packed; + struct diag_ctrl_set_wq_val { uint32_t pkt_id; uint32_t len; @@ -265,6 +299,16 @@ struct diag_ctrl_set_wq_val { uint8_t low_wm_val; } __packed; +struct diag_ctrl_set_wq_val_v2 { + uint32_t pkt_id; + uint32_t len; + uint32_t version; + uint8_t diag_id; + uint8_t stream_id; + uint8_t high_wm_val; + uint8_t low_wm_val; +} __packed; + int diagfwd_cntl_init(void); void diagfwd_cntl_channel_init(void); void diagfwd_cntl_exit(void); @@ -273,14 +317,16 @@ void diag_cntl_channel_close(struct diagfwd_info *p_info); void diag_cntl_process_read_data(struct diagfwd_info *p_info, void *buf, int len); int diag_send_real_time_update(uint8_t peripheral, int real_time); +void diag_map_pd_to_diagid(uint8_t pd, uint8_t *diag_id, int *peripheral); int diag_send_peripheral_buffering_mode(struct diag_buffering_mode_t *params); void diag_update_proc_vote(uint16_t proc, uint8_t vote, int index); void diag_update_real_time_vote(uint16_t proc, uint8_t real_time, int index); void diag_real_time_work_fn(struct work_struct *work); int diag_send_stm_state(uint8_t peripheral, uint8_t stm_control_data); -int diag_send_peripheral_drain_immediate(uint8_t peripheral); +int diag_send_peripheral_drain_immediate(uint8_t pd, + uint8_t diag_id, int peripheral); int diag_send_buffering_tx_mode_pkt(uint8_t peripheral, - struct diag_buffering_mode_t *params); + uint8_t diag_id, struct diag_buffering_mode_t *params); int diag_send_buffering_wm_values(uint8_t peripheral, - struct diag_buffering_mode_t *params); + uint8_t diag_id, struct diag_buffering_mode_t *params); #endif