diag: Add mdlog session to apps pkt response path
Apps pkt cmd response defaults to APPS_DATA peripheral.
Error response for mdlogging does not go to the right md
session. Pass md session along response path to ensure
correct destination.
CRs-Fixed: 2000108
Change-Id: Iece75fdb580a68350085bf48df374e60c2b4adb6
Signed-off-by: Chris Lew <clew@codeaurora.org>
Signed-off-by: Manoj Prabhu B <bmanoj@codeaurora.org>
diff --git a/drivers/char/diag/diagfwd.c b/drivers/char/diag/diagfwd.c
index b59f245..fc67c1a 100644
--- a/drivers/char/diag/diagfwd.c
+++ b/drivers/char/diag/diagfwd.c
@@ -240,10 +240,11 @@ void chk_logging_wakeup(void)
}
}
-static void pack_rsp_and_send(unsigned char *buf, int len)
+static void pack_rsp_and_send(unsigned char *buf, int len,
+ struct diag_md_session_t *info)
{
int err;
- int retry_count = 0;
+ int retry_count = 0, i, rsp_ctxt;
uint32_t write_len = 0;
unsigned long flags;
unsigned char *rsp_ptr = driver->encoded_rsp_buf;
@@ -259,6 +260,26 @@ static void pack_rsp_and_send(unsigned char *buf, int len)
}
/*
+ * Explicitly check for the Peripheral Modem here
+ * is necessary till a way to identify a peripheral
+ * if its supporting qshrink4 feature.
+ */
+ if (info && info->peripheral_mask) {
+ if (info->peripheral_mask == DIAG_CON_ALL ||
+ (info->peripheral_mask & (1 << APPS_DATA)) ||
+ (info->peripheral_mask & (1 << PERIPHERAL_MODEM))) {
+ rsp_ctxt = SET_BUF_CTXT(APPS_DATA, TYPE_CMD, 1);
+ } else {
+ for (i = 0; i < NUM_MD_SESSIONS; i++) {
+ if (info->peripheral_mask & (1 << i))
+ break;
+ }
+ rsp_ctxt = SET_BUF_CTXT(i, TYPE_CMD, 1);
+ }
+ } else
+ rsp_ctxt = driver->rsp_buf_ctxt;
+
+ /*
* Keep trying till we get the buffer back. It should probably
* take one or two iterations. When this loops till UINT_MAX, it
* means we did not get a write complete for the previous
@@ -299,8 +320,7 @@ static void pack_rsp_and_send(unsigned char *buf, int len)
*(uint8_t *)(rsp_ptr + write_len) = CONTROL_CHAR;
write_len += sizeof(uint8_t);
- err = diag_mux_write(DIAG_LOCAL_PROC, rsp_ptr, write_len,
- driver->rsp_buf_ctxt);
+ err = diag_mux_write(DIAG_LOCAL_PROC, rsp_ptr, write_len, rsp_ctxt);
if (err) {
pr_err("diag: In %s, unable to write to mux, err: %d\n",
__func__, err);
@@ -310,12 +330,13 @@ static void pack_rsp_and_send(unsigned char *buf, int len)
}
}
-static void encode_rsp_and_send(unsigned char *buf, int len)
+static void encode_rsp_and_send(unsigned char *buf, int len,
+ struct diag_md_session_t *info)
{
struct diag_send_desc_type send = { NULL, NULL, DIAG_STATE_START, 0 };
struct diag_hdlc_dest_type enc = { NULL, NULL, 0 };
unsigned char *rsp_ptr = driver->encoded_rsp_buf;
- int err, retry_count = 0;
+ int err, i, rsp_ctxt, retry_count = 0;
unsigned long flags;
if (!rsp_ptr || !buf)
@@ -328,6 +349,26 @@ static void encode_rsp_and_send(unsigned char *buf, int len)
}
/*
+ * Explicitly check for the Peripheral Modem here
+ * is necessary till a way to identify a peripheral
+ * if its supporting qshrink4 feature.
+ */
+ if (info && info->peripheral_mask) {
+ if (info->peripheral_mask == DIAG_CON_ALL ||
+ (info->peripheral_mask & (1 << APPS_DATA)) ||
+ (info->peripheral_mask & (1 << PERIPHERAL_MODEM))) {
+ rsp_ctxt = SET_BUF_CTXT(APPS_DATA, TYPE_CMD, 1);
+ } else {
+ for (i = 0; i < NUM_MD_SESSIONS; i++) {
+ if (info->peripheral_mask & (1 << i))
+ break;
+ }
+ rsp_ctxt = SET_BUF_CTXT(i, TYPE_CMD, 1);
+ }
+ } else
+ rsp_ctxt = driver->rsp_buf_ctxt;
+
+ /*
* Keep trying till we get the buffer back. It should probably
* take one or two iterations. When this loops till UINT_MAX, it
* means we did not get a write complete for the previous
@@ -370,7 +411,7 @@ static void encode_rsp_and_send(unsigned char *buf, int len)
diag_hdlc_encode(&send, &enc);
driver->encoded_rsp_len = (int)(enc.dest - (void *)rsp_ptr);
err = diag_mux_write(DIAG_LOCAL_PROC, rsp_ptr, driver->encoded_rsp_len,
- driver->rsp_buf_ctxt);
+ rsp_ctxt);
if (err) {
pr_err("diag: In %s, Unable to write to device, err: %d\n",
__func__, err);
@@ -381,21 +422,23 @@ static void encode_rsp_and_send(unsigned char *buf, int len)
memset(buf, '\0', DIAG_MAX_RSP_SIZE);
}
-void diag_send_rsp(unsigned char *buf, int len)
+static void diag_send_rsp(unsigned char *buf, int len,
+ struct diag_md_session_t *info)
{
struct diag_md_session_t *session_info = NULL;
uint8_t hdlc_disabled;
- session_info = diag_md_session_get_peripheral(APPS_DATA);
+ session_info = (info) ? info :
+ diag_md_session_get_peripheral(APPS_DATA);
if (session_info)
hdlc_disabled = session_info->hdlc_disabled;
else
hdlc_disabled = driver->hdlc_disabled;
if (hdlc_disabled)
- pack_rsp_and_send(buf, len);
+ pack_rsp_and_send(buf, len, session_info);
else
- encode_rsp_and_send(buf, len);
+ encode_rsp_and_send(buf, len, session_info);
}
void diag_update_pkt_buffer(unsigned char *buf, uint32_t len, int type)
@@ -926,7 +969,8 @@ static int diag_cmd_disable_hdlc(unsigned char *src_buf, int src_len,
return write_len;
}
-void diag_send_error_rsp(unsigned char *buf, int len)
+void diag_send_error_rsp(unsigned char *buf, int len,
+ struct diag_md_session_t *info)
{
/* -1 to accommodate the first byte 0x13 */
if (len > (DIAG_MAX_RSP_SIZE - 1)) {
@@ -936,7 +980,7 @@ void diag_send_error_rsp(unsigned char *buf, int len)
*(uint8_t *)driver->apps_rsp_buf = DIAG_CMD_ERROR;
memcpy((driver->apps_rsp_buf + sizeof(uint8_t)), buf, len);
- diag_send_rsp(driver->apps_rsp_buf, len + 1);
+ diag_send_rsp(driver->apps_rsp_buf, len + 1, info);
}
int diag_process_apps_pkt(unsigned char *buf, int len,
@@ -956,7 +1000,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
/* Check if the command is a supported mask command */
mask_ret = diag_process_apps_masks(buf, len, info);
if (mask_ret > 0) {
- diag_send_rsp(driver->apps_rsp_buf, mask_ret);
+ diag_send_rsp(driver->apps_rsp_buf, mask_ret, info);
return 0;
}
@@ -978,7 +1022,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
driver->apps_rsp_buf,
DIAG_MAX_RSP_SIZE);
if (write_len > 0)
- diag_send_rsp(driver->apps_rsp_buf, write_len);
+ diag_send_rsp(driver->apps_rsp_buf, write_len, info);
return 0;
}
@@ -988,13 +1032,15 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
reg_item = container_of(temp_entry, struct diag_cmd_reg_t,
entry);
if (info) {
- if (MD_PERIPHERAL_MASK(reg_item->proc) &
- info->peripheral_mask)
+ if ((MD_PERIPHERAL_MASK(reg_item->proc) &
+ info->peripheral_mask) ||
+ (MD_PERIPHERAL_PD_MASK(reg_item->proc) &
+ info->peripheral_mask))
write_len = diag_send_data(reg_item, buf, len);
} else {
if (MD_PERIPHERAL_MASK(reg_item->proc) &
driver->logging_mask)
- diag_send_error_rsp(buf, len);
+ diag_send_error_rsp(buf, len, info);
else
write_len = diag_send_data(reg_item, buf, len);
}
@@ -1010,13 +1056,13 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
for (i = 0; i < 4; i++)
*(driver->apps_rsp_buf+i) = *(buf+i);
*(uint32_t *)(driver->apps_rsp_buf+4) = DIAG_MAX_REQ_SIZE;
- diag_send_rsp(driver->apps_rsp_buf, 8);
+ diag_send_rsp(driver->apps_rsp_buf, 8, info);
return 0;
} else if ((*buf == 0x4b) && (*(buf+1) == 0x12) &&
(*(uint16_t *)(buf+2) == DIAG_DIAG_STM)) {
len = diag_process_stm_cmd(buf, driver->apps_rsp_buf);
if (len > 0) {
- diag_send_rsp(driver->apps_rsp_buf, len);
+ diag_send_rsp(driver->apps_rsp_buf, len, info);
return 0;
}
return len;
@@ -1029,7 +1075,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
driver->apps_rsp_buf,
DIAG_MAX_RSP_SIZE);
if (write_len > 0)
- diag_send_rsp(driver->apps_rsp_buf, write_len);
+ diag_send_rsp(driver->apps_rsp_buf, write_len, info);
return 0;
}
/* Check for time sync switch command */
@@ -1040,7 +1086,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
driver->apps_rsp_buf,
DIAG_MAX_RSP_SIZE);
if (write_len > 0)
- diag_send_rsp(driver->apps_rsp_buf, write_len);
+ diag_send_rsp(driver->apps_rsp_buf, write_len, info);
return 0;
}
/* Check for diag id command */
@@ -1051,14 +1097,14 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
driver->apps_rsp_buf,
DIAG_MAX_RSP_SIZE);
if (write_len > 0)
- diag_send_rsp(driver->apps_rsp_buf, write_len);
+ diag_send_rsp(driver->apps_rsp_buf, write_len, info);
return 0;
}
/* Check for download command */
else if ((chk_apps_master()) && (*buf == 0x3A)) {
/* send response back */
driver->apps_rsp_buf[0] = *buf;
- diag_send_rsp(driver->apps_rsp_buf, 1);
+ diag_send_rsp(driver->apps_rsp_buf, 1, info);
msleep(5000);
/* call download API */
msm_set_restart_mode(RESTART_DLOAD);
@@ -1078,7 +1124,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
for (i = 0; i < 13; i++)
driver->apps_rsp_buf[i+3] = 0;
- diag_send_rsp(driver->apps_rsp_buf, 16);
+ diag_send_rsp(driver->apps_rsp_buf, 16, info);
return 0;
}
}
@@ -1087,7 +1133,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
(*(buf+2) == 0x04) && (*(buf+3) == 0x0)) {
memcpy(driver->apps_rsp_buf, buf, 4);
driver->apps_rsp_buf[4] = wrap_enabled;
- diag_send_rsp(driver->apps_rsp_buf, 5);
+ diag_send_rsp(driver->apps_rsp_buf, 5, info);
return 0;
}
/* Wrap the Delayed Rsp ID */
@@ -1096,7 +1142,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
wrap_enabled = true;
memcpy(driver->apps_rsp_buf, buf, 4);
driver->apps_rsp_buf[4] = wrap_count;
- diag_send_rsp(driver->apps_rsp_buf, 6);
+ diag_send_rsp(driver->apps_rsp_buf, 6, info);
return 0;
}
/* Mobile ID Rsp */
@@ -1107,7 +1153,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
driver->apps_rsp_buf,
DIAG_MAX_RSP_SIZE);
if (write_len > 0) {
- diag_send_rsp(driver->apps_rsp_buf, write_len);
+ diag_send_rsp(driver->apps_rsp_buf, write_len, info);
return 0;
}
}
@@ -1127,7 +1173,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
for (i = 0; i < 55; i++)
driver->apps_rsp_buf[i] = 0;
- diag_send_rsp(driver->apps_rsp_buf, 55);
+ diag_send_rsp(driver->apps_rsp_buf, 55, info);
return 0;
}
/* respond to 0x7c command */
@@ -1140,14 +1186,14 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
chk_config_get_id();
*(unsigned char *)(driver->apps_rsp_buf + 12) = '\0';
*(unsigned char *)(driver->apps_rsp_buf + 13) = '\0';
- diag_send_rsp(driver->apps_rsp_buf, 14);
+ diag_send_rsp(driver->apps_rsp_buf, 14, info);
return 0;
}
}
write_len = diag_cmd_chk_stats(buf, len, driver->apps_rsp_buf,
DIAG_MAX_RSP_SIZE);
if (write_len > 0) {
- diag_send_rsp(driver->apps_rsp_buf, write_len);
+ diag_send_rsp(driver->apps_rsp_buf, write_len, info);
return 0;
}
write_len = diag_cmd_disable_hdlc(buf, len, driver->apps_rsp_buf,
@@ -1159,7 +1205,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
* before disabling HDLC encoding on Apps processor.
*/
mutex_lock(&driver->hdlc_disable_mutex);
- diag_send_rsp(driver->apps_rsp_buf, write_len);
+ diag_send_rsp(driver->apps_rsp_buf, write_len, info);
/*
* Set the value of hdlc_disabled after sending the response to
* the tools. This is required since the tools is expecting a
@@ -1179,7 +1225,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
/* We have now come to the end of the function. */
if (chk_apps_only())
- diag_send_error_rsp(buf, len);
+ diag_send_error_rsp(buf, len, info);
return 0;
}
@@ -1262,7 +1308,7 @@ void diag_process_hdlc_pkt(void *data, unsigned int len,
* recovery algorithm. Send an error response if the
* packet is not in expected format.
*/
- diag_send_error_rsp(driver->hdlc_buf, driver->hdlc_buf_len);
+ diag_send_error_rsp(driver->hdlc_buf, driver->hdlc_buf_len, info);
driver->hdlc_buf_len = 0;
end:
mutex_unlock(&driver->diag_hdlc_mutex);
@@ -1535,7 +1581,7 @@ void diag_process_non_hdlc_pkt(unsigned char *buf, int len,
if (actual_pkt->start != CONTROL_CHAR) {
diag_hdlc_start_recovery(buf, len, info);
- diag_send_error_rsp(buf, len);
+ diag_send_error_rsp(buf, len, info);
goto end;
}
mutex_lock(&driver->hdlc_recovery_mutex);
@@ -1625,15 +1671,14 @@ static int diagfwd_mux_write_done(unsigned char *buf, int len, int buf_ctxt,
case TYPE_CMD:
if (peripheral >= 0 && peripheral < NUM_PERIPHERALS) {
diagfwd_write_done(peripheral, type, num);
- } else if (peripheral == APPS_DATA) {
+ }
+ if (peripheral == APPS_DATA ||
+ ctxt == DIAG_MEMORY_DEVICE_MODE) {
spin_lock_irqsave(&driver->rsp_buf_busy_lock, flags);
driver->rsp_buf_busy = 0;
driver->encoded_rsp_len = 0;
spin_unlock_irqrestore(&driver->rsp_buf_busy_lock,
flags);
- } else {
- pr_err_ratelimited("diag: Invalid peripheral %d in %s, type: %d\n",
- peripheral, __func__, type);
}
break;
default: