diag: Add apps diag support for STM
Add apps diag support for recording of apps diag
data (F3s, logs, and events) to STM.
Change-Id: I3753033c70d92e0d581eb2827c6efa342166c120
Signed-off-by: Dixon Peterson <dixonp@codeaurora.org>
diff --git a/drivers/char/diag/diag_debugfs.c b/drivers/char/diag/diag_debugfs.c
index 81c5e57..0badda6 100644
--- a/drivers/char/diag/diag_debugfs.c
+++ b/drivers/char/diag/diag_debugfs.c
@@ -70,6 +70,17 @@
"Modem CMD in_busy_1: %d\n"
"Modem CMD in_busy_2: %d\n"
"DCI CMD Modem in_busy_1: %d\n"
+ "Modem supports STM: %d\n"
+ "LPASS supports STM: %d\n"
+ "RIVA supports STM: %d\n"
+ "Modem STM state: %d\n"
+ "LPASS STM state: %d\n"
+ "RIVA STM state: %d\n"
+ "APPS STM state: %d\n"
+ "Modem STM requested state: %d\n"
+ "LPASS STM requested state: %d\n"
+ "RIVA STM requested state: %d\n"
+ "APPS STM requested state: %d\n"
"logging_mode: %d\n"
"real_time_mode: %d\n",
(unsigned int)driver->smd_data[MODEM_DATA].ch,
@@ -101,6 +112,17 @@
driver->smd_cmd[MODEM_DATA].in_busy_1,
driver->smd_cmd[MODEM_DATA].in_busy_2,
driver->smd_dci_cmd[MODEM_DATA].in_busy_1,
+ driver->peripheral_supports_stm[MODEM_DATA],
+ driver->peripheral_supports_stm[LPASS_DATA],
+ driver->peripheral_supports_stm[WCNSS_DATA],
+ driver->stm_state[MODEM_DATA],
+ driver->stm_state[LPASS_DATA],
+ driver->stm_state[WCNSS_DATA],
+ driver->stm_state[APPS_DATA],
+ driver->stm_state_requested[MODEM_DATA],
+ driver->stm_state_requested[LPASS_DATA],
+ driver->stm_state_requested[WCNSS_DATA],
+ driver->stm_state_requested[APPS_DATA],
driver->logging_mode,
driver->real_time_mode);
diff --git a/drivers/char/diag/diag_masks.c b/drivers/char/diag/diag_masks.c
index d838714..aa1d847 100644
--- a/drivers/char/diag/diag_masks.c
+++ b/drivers/char/diag/diag_masks.c
@@ -26,7 +26,7 @@
#define ALL_SSID -1
#define MAX_SSID_PER_RANGE 100
-#define FEATURE_MASK_LEN_BYTES 1
+#define FEATURE_MASK_LEN_BYTES 2
struct mask_info {
int equip_id;
@@ -466,7 +466,7 @@
void *buf = driver->buf_feature_mask_update;
int header_size = sizeof(struct diag_ctrl_feature_mask);
int wr_size = -ENOMEM, retry_count = 0;
- uint8_t feature_byte = 0;
+ uint8_t feature_bytes[FEATURE_MASK_LEN_BYTES] = {0, 0};
int total_len = 0;
if (!smd_info) {
@@ -487,11 +487,12 @@
driver->feature_mask->ctrl_pkt_data_len = 4 + FEATURE_MASK_LEN_BYTES;
driver->feature_mask->feature_mask_len = FEATURE_MASK_LEN_BYTES;
memcpy(buf, driver->feature_mask, header_size);
- feature_byte |= F_DIAG_INT_FEATURE_MASK;
- feature_byte |= F_DIAG_LOG_ON_DEMAND_RSP_ON_MASTER;
- feature_byte |= driver->supports_separate_cmdrsp ?
+ feature_bytes[0] |= F_DIAG_INT_FEATURE_MASK;
+ feature_bytes[0] |= F_DIAG_LOG_ON_DEMAND_RSP_ON_MASTER;
+ feature_bytes[0] |= driver->supports_separate_cmdrsp ?
F_DIAG_REQ_RSP_CHANNEL : 0;
- memcpy(buf+header_size, &feature_byte, FEATURE_MASK_LEN_BYTES);
+ feature_bytes[1] |= F_DIAG_OVER_STM;
+ memcpy(buf+header_size, &feature_bytes, FEATURE_MASK_LEN_BYTES);
total_len = header_size + FEATURE_MASK_LEN_BYTES;
while (retry_count < 3) {
diff --git a/drivers/char/diag/diagchar.h b/drivers/char/diag/diagchar.h
index 1495ad5..b7784b5 100644
--- a/drivers/char/diag/diagchar.h
+++ b/drivers/char/diag/diagchar.h
@@ -89,6 +89,13 @@
#define DIAG_CON_LPASS (0x0004) /* Bit mask for LPASS */
#define DIAG_CON_WCNSS (0x0008) /* Bit mask for WCNSS */
+#define NUM_STM_PROCESSORS 4
+
+#define DIAG_STM_MODEM 0x01
+#define DIAG_STM_LPASS 0x02
+#define DIAG_STM_WCNSS 0x04
+#define DIAG_STM_APPS 0x08
+
/*
* The status bit masks when received in a signal handler are to be
* used in conjunction with the peripheral list bit mask to determine the
@@ -230,6 +237,8 @@
struct work_struct diag_read_smd_work;
struct work_struct diag_notify_update_smd_work;
int notify_context;
+ struct work_struct diag_general_smd_work;
+ int general_context;
/*
* Function ptr for function to call to process the data that
@@ -261,6 +270,12 @@
unsigned int buf_tbl_size;
int use_device_tree;
int supports_separate_cmdrsp;
+ /* The state requested in the STM command */
+ int stm_state_requested[NUM_STM_PROCESSORS];
+ /* The current STM state */
+ int stm_state[NUM_STM_PROCESSORS];
+ /* Whether or not the peripheral supports STM */
+ int peripheral_supports_stm[NUM_SMD_CONTROL_CHANNELS];
/* DCI related variables */
struct dci_pkt_req_tracking_tbl *req_tracking_tbl;
struct diag_dci_client_tbl *dci_client_tbl;
diff --git a/drivers/char/diag/diagchar_core.c b/drivers/char/diag/diagchar_core.c
index 12c40da..7513c7b 100644
--- a/drivers/char/diag/diagchar_core.c
+++ b/drivers/char/diag/diagchar_core.c
@@ -44,6 +44,9 @@
#include "diag_masks.h"
#include "diagfwd_bridge.h"
+#include <linux/coresight-stm.h>
+#include <linux/kernel.h>
+
MODULE_DESCRIPTION("Diag Char Driver");
MODULE_LICENSE("GPL v2");
MODULE_VERSION("1.0");
@@ -1797,6 +1800,21 @@
ret = -EFAULT;
goto fail_free_copy;
}
+ if (driver->stm_state[APPS_DATA] &&
+ (pkt_type >= DATA_TYPE_EVENT && pkt_type <= DATA_TYPE_LOG)) {
+ int stm_size = 0;
+
+ stm_size = stm_log_inv_ts(OST_ENTITY_DIAG, 0, buf_copy,
+ payload_size);
+
+ if (stm_size == 0)
+ pr_debug("diag: In %s, stm_log_inv_ts returned size of 0\n",
+ __func__);
+
+ diagmem_free(driver, buf_copy, POOL_TYPE_COPY);
+ return 0;
+ }
+
#ifdef DIAG_DEBUG
printk(KERN_DEBUG "data is -->\n");
for (i = 0; i < payload_size; i++)
diff --git a/drivers/char/diag/diagfwd.c b/drivers/char/diag/diagfwd.c
index d07cc04..6e7080e 100644
--- a/drivers/char/diag/diagfwd.c
+++ b/drivers/char/diag/diagfwd.c
@@ -46,6 +46,16 @@
#define MODE_CMD 41
#define RESET_ID 2
+#define STM_CMD_VERSION_OFFSET 4
+#define STM_CMD_MASK_OFFSET 5
+#define STM_CMD_DATA_OFFSET 6
+#define STM_CMD_NUM_BYTES 7
+
+#define STM_RSP_VALID_INDEX 7
+#define STM_RSP_SUPPORTED_INDEX 8
+#define STM_RSP_SMD_COMPLY_INDEX 9
+#define STM_RSP_NUM_BYTES 10
+
int diag_debug_buf_idx;
unsigned char diag_debug_buf[1024];
/* Number of entries in table of buffers */
@@ -763,6 +773,77 @@
}
}
+void diag_process_stm_mask(uint8_t cmd, uint8_t data_mask, int data_type,
+ uint8_t *rsp_supported, uint8_t *rsp_smd_comply)
+{
+ int status = 0;
+ if (data_type >= MODEM_DATA && data_type <= WCNSS_DATA) {
+ if (driver->peripheral_supports_stm[data_type]) {
+ status = diag_send_stm_state(
+ &driver->smd_cntl[data_type], cmd);
+ if (status == 1)
+ *rsp_smd_comply |= data_mask;
+ *rsp_supported |= data_mask;
+ } else if (driver->smd_cntl[data_type].ch) {
+ *rsp_smd_comply |= data_mask;
+ }
+ if ((*rsp_smd_comply & data_mask) &&
+ (*rsp_supported & data_mask))
+ driver->stm_state[data_type] = cmd;
+
+ driver->stm_state_requested[data_type] = cmd;
+ } else if (data_type == APPS_DATA) {
+ *rsp_supported |= data_mask;
+ *rsp_smd_comply |= data_mask;
+ driver->stm_state[data_type] = cmd;
+ driver->stm_state_requested[data_type] = cmd;
+ }
+}
+
+int diag_process_stm_cmd(unsigned char *buf)
+{
+ uint8_t version = *(buf+STM_CMD_VERSION_OFFSET);
+ uint8_t mask = *(buf+STM_CMD_MASK_OFFSET);
+ uint8_t cmd = *(buf+STM_CMD_DATA_OFFSET);
+ uint8_t rsp_supported = 0;
+ uint8_t rsp_smd_comply = 0;
+ int valid_command = 1;
+ int i;
+
+ /* Check if command is valid */
+ if ((version != 1) || (mask == 0) || (0 != (mask >> 4)) ||
+ (cmd != ENABLE_STM && cmd != DISABLE_STM)) {
+ valid_command = 0;
+ } else {
+ if (mask & DIAG_STM_MODEM)
+ diag_process_stm_mask(cmd, DIAG_STM_MODEM, MODEM_DATA,
+ &rsp_supported, &rsp_smd_comply);
+
+ if (mask & DIAG_STM_LPASS)
+ diag_process_stm_mask(cmd, DIAG_STM_LPASS, LPASS_DATA,
+ &rsp_supported, &rsp_smd_comply);
+
+ if (mask & DIAG_STM_WCNSS)
+ diag_process_stm_mask(cmd, DIAG_STM_WCNSS, WCNSS_DATA,
+ &rsp_supported, &rsp_smd_comply);
+
+ if (mask & DIAG_STM_APPS)
+ diag_process_stm_mask(cmd, DIAG_STM_APPS, APPS_DATA,
+ &rsp_supported, &rsp_smd_comply);
+ }
+
+ for (i = 0; i < STM_CMD_NUM_BYTES; i++)
+ driver->apps_rsp_buf[i] = *(buf+i);
+
+ driver->apps_rsp_buf[STM_RSP_VALID_INDEX] = valid_command;
+ driver->apps_rsp_buf[STM_RSP_SUPPORTED_INDEX] = rsp_supported;
+ driver->apps_rsp_buf[STM_RSP_SMD_COMPLY_INDEX] = rsp_smd_comply;
+
+ encode_rsp_and_send(STM_RSP_NUM_BYTES-1);
+
+ return 0;
+}
+
int diag_process_apps_pkt(unsigned char *buf, int len)
{
uint16_t subsys_cmd_code;
@@ -838,6 +919,9 @@
*(uint32_t *)(driver->apps_rsp_buf+4) = PKT_SIZE;
encode_rsp_and_send(7);
return 0;
+ } else if ((*buf == 0x4b) && (*(buf+1) == 0x12) &&
+ (*(uint16_t *)(buf+2) == 0x020E)) {
+ return diag_process_stm_cmd(buf);
}
/* Check for Apps Only & get event mask request */
else if (!(driver->smd_data[MODEM_DATA].ch) && chk_apps_only() &&
@@ -1592,6 +1676,9 @@
/* Notify the clients of the close */
diag_dci_notify_client(smd_info->peripheral_mask,
DIAG_STATUS_CLOSED);
+ } else if (smd_info->type == SMD_CNTL_TYPE) {
+ diag_cntl_stm_notify(smd_info,
+ CLEAR_PERIPHERAL_STM_STATE);
}
return;
} else if (event == SMD_EVENT_OPEN) {
@@ -1833,20 +1920,27 @@
* information to the update function.
*/
smd_info->notify_context = 0;
+ smd_info->general_context = 0;
switch (type) {
case SMD_DATA_TYPE:
case SMD_CMD_TYPE:
INIT_WORK(&(smd_info->diag_notify_update_smd_work),
diag_clean_reg_fn);
+ INIT_WORK(&(smd_info->diag_general_smd_work),
+ diag_cntl_smd_work_fn);
break;
case SMD_CNTL_TYPE:
INIT_WORK(&(smd_info->diag_notify_update_smd_work),
diag_mask_update_fn);
+ INIT_WORK(&(smd_info->diag_general_smd_work),
+ diag_cntl_smd_work_fn);
break;
case SMD_DCI_TYPE:
case SMD_DCI_CMD_TYPE:
INIT_WORK(&(smd_info->diag_notify_update_smd_work),
diag_update_smd_dci_work_fn);
+ INIT_WORK(&(smd_info->diag_general_smd_work),
+ diag_cntl_smd_work_fn);
break;
default:
pr_err("diag: In %s, unknown type, type: %d\n", __func__, type);
@@ -1931,8 +2025,15 @@
mutex_init(&driver->diag_hdlc_mutex);
mutex_init(&driver->diag_cntl_mutex);
- for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++)
+ for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
driver->separate_cmdrsp[i] = 0;
+ driver->peripheral_supports_stm[i] = DISABLE_STM;
+ }
+
+ for (i = 0; i < NUM_STM_PROCESSORS; i++) {
+ driver->stm_state_requested[i] = DISABLE_STM;
+ driver->stm_state[i] = DISABLE_STM;
+ }
for (i = 0; i < NUM_SMD_DATA_CHANNELS; i++) {
success = diag_smd_constructor(&driver->smd_data[i], i,
diff --git a/drivers/char/diag/diagfwd_cntl.c b/drivers/char/diag/diagfwd_cntl.c
index b36b7dd..a832cb3 100644
--- a/drivers/char/diag/diagfwd_cntl.c
+++ b/drivers/char/diag/diagfwd_cntl.c
@@ -40,6 +40,55 @@
smd_info->notify_context = 0;
}
+void diag_cntl_smd_work_fn(struct work_struct *work)
+{
+ struct diag_smd_info *smd_info = container_of(work,
+ struct diag_smd_info,
+ diag_general_smd_work);
+
+ if (!smd_info || smd_info->type != SMD_CNTL_TYPE)
+ return;
+
+ if (smd_info->general_context == UPDATE_PERIPHERAL_STM_STATE) {
+ if (driver->peripheral_supports_stm[smd_info->peripheral] ==
+ ENABLE_STM) {
+ int status = 0;
+ int index = smd_info->peripheral;
+ status = diag_send_stm_state(smd_info,
+ (uint8_t)(driver->stm_state_requested[index]));
+ if (status == 1)
+ driver->stm_state[index] =
+ driver->stm_state_requested[index];
+ }
+ }
+ smd_info->general_context = 0;
+}
+
+void diag_cntl_stm_notify(struct diag_smd_info *smd_info, int action)
+{
+ if (!smd_info || smd_info->type != SMD_CNTL_TYPE)
+ return;
+
+ if (action == CLEAR_PERIPHERAL_STM_STATE)
+ driver->peripheral_supports_stm[smd_info->peripheral] =
+ DISABLE_STM;
+}
+
+static void process_stm_feature(struct diag_smd_info *smd_info,
+ uint8_t feature_mask)
+{
+ if (feature_mask & F_DIAG_OVER_STM) {
+ driver->peripheral_supports_stm[smd_info->peripheral] =
+ ENABLE_STM;
+ smd_info->general_context = UPDATE_PERIPHERAL_STM_STATE;
+ queue_work(driver->diag_cntl_wq,
+ &(smd_info->diag_general_smd_work));
+ } else {
+ driver->peripheral_supports_stm[smd_info->peripheral] =
+ DISABLE_STM;
+ }
+}
+
/* Process the data read from the smd control channel */
int diag_process_smd_cntl_read_data(struct diag_smd_info *smd_info, void *buf,
int total_recd)
@@ -120,8 +169,9 @@
uint8_t feature_mask = 0;
int feature_mask_len = *(int *)(buf+8);
if (feature_mask_len > 0) {
+ int periph = smd_info->peripheral;
feature_mask = *(uint8_t *)(buf+12);
- if (smd_info->peripheral == MODEM_DATA)
+ if (periph == MODEM_DATA)
driver->log_on_demand_support =
feature_mask &
F_DIAG_LOG_ON_DEMAND_RSP_ON_MASTER;
@@ -132,13 +182,16 @@
*/
if (driver->supports_separate_cmdrsp &&
(feature_mask & F_DIAG_REQ_RSP_CHANNEL))
- driver->separate_cmdrsp
- [smd_info->peripheral] =
+ driver->separate_cmdrsp[periph] =
ENABLE_SEPARATE_CMDRSP;
else
- driver->separate_cmdrsp
- [smd_info->peripheral] =
+ driver->separate_cmdrsp[periph] =
DISABLE_SEPARATE_CMDRSP;
+ if (feature_mask_len > 1) {
+ feature_mask = *(uint8_t *)(buf+13);
+ process_stm_feature(smd_info,
+ feature_mask);
+ }
}
flag = 1;
} else if (type != DIAG_CTRL_MSG_REG) {
@@ -298,6 +351,56 @@
mutex_unlock(&driver->diag_cntl_mutex);
}
+int diag_send_stm_state(struct diag_smd_info *smd_info,
+ uint8_t stm_control_data)
+{
+ struct diag_ctrl_msg_stm stm_msg;
+ int msg_size = sizeof(struct diag_ctrl_msg_stm);
+ int retry_count = 0;
+ int wr_size = 0;
+ int success = 0;
+
+ if (!smd_info || (smd_info->type != SMD_CNTL_TYPE) ||
+ (driver->peripheral_supports_stm[smd_info->peripheral] ==
+ DISABLE_STM)) {
+ return -EINVAL;
+ }
+
+ if (smd_info->ch) {
+ stm_msg.ctrl_pkt_id = 21;
+ stm_msg.ctrl_pkt_data_len = 5;
+ stm_msg.version = 1;
+ stm_msg.control_data = stm_control_data;
+ while (retry_count < 3) {
+ wr_size = smd_write(smd_info->ch, &stm_msg, msg_size);
+ if (wr_size == -ENOMEM) {
+ /*
+ * The smd channel is full. Delay while
+ * smd processes existing data and smd
+ * has memory become available. The delay
+ * of 10000 was determined empirically as
+ * best value to use.
+ */
+ retry_count++;
+ usleep_range(10000, 10000);
+ } else {
+ success = 1;
+ break;
+ }
+ }
+ if (wr_size != msg_size) {
+ pr_err("diag: In %s, proc %d fail STM update %d, tried %d",
+ __func__, smd_info->peripheral, wr_size,
+ msg_size);
+ success = 0;
+ }
+ } else {
+ pr_err("diag: In %s, ch invalid, STM update on proc %d\n",
+ __func__, smd_info->peripheral);
+ }
+ return success;
+}
+
static int diag_smd_cntl_probe(struct platform_device *pdev)
{
int r = 0;
diff --git a/drivers/char/diag/diagfwd_cntl.h b/drivers/char/diag/diagfwd_cntl.h
index ddefe10..c90c132 100644
--- a/drivers/char/diag/diagfwd_cntl.h
+++ b/drivers/char/diag/diagfwd_cntl.h
@@ -45,10 +45,18 @@
* new Data Rx and DCI Rx channels
*/
#define F_DIAG_REQ_RSP_CHANNEL 0x10
+/* Denotes we support diag over stm */
+#define F_DIAG_OVER_STM 0x02
#define ENABLE_SEPARATE_CMDRSP 1
#define DISABLE_SEPARATE_CMDRSP 0
+#define ENABLE_STM 1
+#define DISABLE_STM 0
+
+#define UPDATE_PERIPHERAL_STM_STATE 1
+#define CLEAR_PERIPHERAL_STM_STATE 2
+
struct cmd_code_range {
uint16_t cmd_code_lo;
uint16_t cmd_code_hi;
@@ -117,11 +125,19 @@
uint32_t event_stale_timer_val;
} __packed;
+struct diag_ctrl_msg_stm {
+ uint32_t ctrl_pkt_id;
+ uint32_t ctrl_pkt_data_len;
+ uint32_t version;
+ uint8_t control_data;
+} __packed;
+
void diagfwd_cntl_init(void);
void diagfwd_cntl_exit(void);
void diag_read_smd_cntl_work_fn(struct work_struct *);
void diag_notify_ctrl_update_fn(struct work_struct *work);
void diag_clean_reg_fn(struct work_struct *work);
+void diag_cntl_smd_work_fn(struct work_struct *work);
int diag_process_smd_cntl_read_data(struct diag_smd_info *smd_info, void *buf,
int total_recd);
void diag_send_diag_mode_update_by_smd(struct diag_smd_info *smd_info,
@@ -129,4 +145,8 @@
void diag_update_proc_vote(uint16_t proc, uint8_t vote);
void diag_update_real_time_vote(uint16_t proc, uint8_t real_time);
void diag_real_time_work_fn(struct work_struct *work);
+int diag_send_stm_state(struct diag_smd_info *smd_info,
+ uint8_t stm_control_data);
+void diag_cntl_stm_notify(struct diag_smd_info *smd_info, int action);
+
#endif