diag: Add hdlc encoding support for diag data sent from peripherals
Diag running on the peripherals currently hdlc encodes the diag
data prior to being sent to the APPS processor over the diag data
smd channels. To offload the peripherals of this workload, add support
to accept non-hdlc encoded diag data from the peripherals and
hdlc encode that data on the APPS processor. Maintain the backwards
compatability to accept and properly process hdlc encoded data.
Add ability to determine at run-time whether a peripheral is sending
hdlc or non-hdlc encoded data and process the data appropriately.
Change-Id: I09e1dce0118011e52b1634f1eac402de16a999f6
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 3d1a6cd..3a2a8b2 100644
--- a/drivers/char/diag/diag_debugfs.c
+++ b/drivers/char/diag/diag_debugfs.c
@@ -81,6 +81,11 @@
"LPASS STM requested state: %d\n"
"RIVA STM requested state: %d\n"
"APPS STM requested state: %d\n"
+ "supports apps hdlc encoding: %d\n"
+ "Modem hdlc encoding: %d\n"
+ "Lpass hdlc encoding: %d\n"
+ "RIVA hdlc encoding: %d\n"
+ "Modem CMD hdlc encoding: %d\n"
"logging_mode: %d\n"
"real_time_mode: %d\n",
(unsigned int)driver->smd_data[MODEM_DATA].ch,
@@ -123,6 +128,11 @@
driver->stm_state_requested[LPASS_DATA],
driver->stm_state_requested[WCNSS_DATA],
driver->stm_state_requested[APPS_DATA],
+ driver->supports_apps_hdlc_encoding,
+ driver->smd_data[MODEM_DATA].encode_hdlc,
+ driver->smd_data[LPASS_DATA].encode_hdlc,
+ driver->smd_data[WCNSS_DATA].encode_hdlc,
+ driver->smd_cmd[MODEM_DATA].encode_hdlc,
driver->logging_mode,
driver->real_time_mode);
diff --git a/drivers/char/diag/diag_masks.c b/drivers/char/diag/diag_masks.c
index aa1d847..c91095e 100644
--- a/drivers/char/diag/diag_masks.c
+++ b/drivers/char/diag/diag_masks.c
@@ -491,6 +491,8 @@
feature_bytes[0] |= F_DIAG_LOG_ON_DEMAND_RSP_ON_MASTER;
feature_bytes[0] |= driver->supports_separate_cmdrsp ?
F_DIAG_REQ_RSP_CHANNEL : 0;
+ feature_bytes[0] |= driver->supports_apps_hdlc_encoding ?
+ F_DIAG_HDLC_ENCODE_IN_APPS_MASK : 0;
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;
diff --git a/drivers/char/diag/diagchar.h b/drivers/char/diag/diagchar.h
index 7154942..7953629 100644
--- a/drivers/char/diag/diagchar.h
+++ b/drivers/char/diag/diagchar.h
@@ -217,6 +217,7 @@
int peripheral; /* The peripheral this smd channel communicates with */
int type; /* The type of smd channel (data, control, dci) */
uint16_t peripheral_mask;
+ int encode_hdlc; /* Whether data is raw and needs to be hdlc encoded */
smd_channel_t *ch;
smd_channel_t *ch_save;
@@ -229,6 +230,9 @@
unsigned char *buf_in_1;
unsigned char *buf_in_2;
+ unsigned char *buf_in_1_raw;
+ unsigned char *buf_in_2_raw;
+
struct diag_request *write_ptr_1;
struct diag_request *write_ptr_2;
@@ -270,6 +274,7 @@
unsigned int buf_tbl_size;
int use_device_tree;
int supports_separate_cmdrsp;
+ int supports_apps_hdlc_encoding;
/* The state requested in the STM command */
int stm_state_requested[NUM_STM_PROCESSORS];
/* The current STM state */
diff --git a/drivers/char/diag/diagfwd.c b/drivers/char/diag/diagfwd.c
index 379dc4d..9cf8289 100644
--- a/drivers/char/diag/diagfwd.c
+++ b/drivers/char/diag/diagfwd.c
@@ -61,13 +61,13 @@
/* Number of entries in table of buffers */
static unsigned int buf_tbl_size = 10;
struct diag_master_table entry;
-struct diag_send_desc_type send = { NULL, NULL, DIAG_STATE_START, 0 };
-struct diag_hdlc_dest_type enc = { NULL, NULL, 0 };
int wrap_enabled;
uint16_t wrap_count;
void encode_rsp_and_send(int buf_length)
{
+ struct diag_send_desc_type send = { NULL, NULL, DIAG_STATE_START, 0 };
+ struct diag_hdlc_dest_type enc = { NULL, NULL, 0 };
struct diag_smd_info *data = &(driver->smd_data[MODEM_DATA]);
if (buf_length > APPS_BUF_SIZE) {
@@ -244,6 +244,124 @@
}
}
}
+int diag_add_hdlc_encoding(struct diag_smd_info *smd_info, void *buf,
+ int total_recd, uint8_t *encode_buf,
+ int *encoded_length)
+{
+ struct diag_send_desc_type send = { NULL, NULL, DIAG_STATE_START, 0 };
+ struct diag_hdlc_dest_type enc = { NULL, NULL, 0 };
+ struct data_header {
+ uint8_t control_char;
+ uint8_t version;
+ uint16_t length;
+ };
+ struct data_header *header;
+ int header_size = sizeof(struct data_header);
+ uint8_t *end_control_char;
+ uint8_t *payload;
+ uint8_t *temp_buf;
+ uint8_t *temp_encode_buf;
+ int src_pkt_len;
+ int encoded_pkt_length;
+ int max_size;
+ int total_processed = 0;
+ int bytes_remaining;
+ int success = 1;
+
+ temp_buf = buf;
+ temp_encode_buf = encode_buf;
+ bytes_remaining = *encoded_length;
+ while (total_processed < total_recd) {
+ header = (struct data_header *)temp_buf;
+ /* Perform initial error checking */
+ if (header->control_char != CONTROL_CHAR ||
+ header->version != 1) {
+ success = 0;
+ break;
+ }
+ payload = temp_buf + header_size;
+ end_control_char = payload + header->length;
+ if (*end_control_char != CONTROL_CHAR) {
+ success = 0;
+ break;
+ }
+
+ max_size = 2 * header->length + 3;
+ if (bytes_remaining < max_size) {
+ pr_err("diag: In %s, Not enough room to encode remaining data for peripheral: %d, bytes available: %d, max_size: %d\n",
+ __func__, smd_info->peripheral,
+ bytes_remaining, max_size);
+ success = 0;
+ break;
+ }
+
+ /* Prepare for encoding the data */
+ send.state = DIAG_STATE_START;
+ send.pkt = payload;
+ send.last = (void *)(payload + header->length - 1);
+ send.terminate = 1;
+
+ enc.dest = temp_encode_buf;
+ enc.dest_last = (void *)(temp_encode_buf + max_size);
+ enc.crc = 0;
+ diag_hdlc_encode(&send, &enc);
+
+ /* Prepare for next packet */
+ src_pkt_len = (header_size + header->length + 1);
+ total_processed += src_pkt_len;
+ temp_buf += src_pkt_len;
+
+ encoded_pkt_length = (uint8_t *)enc.dest - temp_encode_buf;
+ bytes_remaining -= encoded_pkt_length;
+ temp_encode_buf = enc.dest;
+ }
+
+ *encoded_length = (int)(temp_encode_buf - encode_buf);
+
+ return success;
+}
+
+static int check_bufsize_for_encoding(struct diag_smd_info *smd_info, void *buf,
+ int total_recd)
+{
+ int buf_size = IN_BUF_SIZE;
+ int max_size = 2 * total_recd + 3;
+ unsigned char *temp_buf;
+
+ if (max_size > IN_BUF_SIZE) {
+ if (max_size < MAX_IN_BUF_SIZE) {
+ pr_err("diag: In %s, SMD sending packet of %d bytes that may expand to %d bytes, peripheral: %d\n",
+ __func__, total_recd, max_size,
+ smd_info->peripheral);
+ if (buf == smd_info->buf_in_1_raw) {
+ temp_buf = krealloc(smd_info->buf_in_1,
+ max_size, GFP_KERNEL);
+ if (temp_buf) {
+ smd_info->buf_in_1 = temp_buf;
+ buf_size = max_size;
+ } else {
+ buf_size = 0;
+ }
+ } else {
+ temp_buf = krealloc(smd_info->buf_in_2,
+ max_size, GFP_KERNEL);
+ if (temp_buf) {
+ smd_info->buf_in_2 = temp_buf;
+ buf_size = max_size;
+ } else {
+ buf_size = 0;
+ }
+ }
+ } else {
+ pr_err("diag: In %s, SMD sending packet of size %d. HDCL encoding can expand to more than %d bytes, peripheral: %d. Discarding.\n",
+ __func__, max_size, MAX_IN_BUF_SIZE,
+ smd_info->peripheral);
+ buf_size = 0;
+ }
+ }
+
+ return buf_size;
+}
void process_lock_enabling(struct diag_nrt_wake_lock *lock, int real_time)
{
@@ -334,7 +452,7 @@
/* Process the data read from the smd data channel */
int diag_process_smd_read_data(struct diag_smd_info *smd_info, void *buf,
- int total_recd)
+ int total_recd)
{
struct diag_request *write_ptr_modem = NULL;
int *in_busy_ptr = 0;
@@ -352,26 +470,74 @@
return 0;
}
- if (smd_info->buf_in_1 == buf) {
- write_ptr_modem = smd_info->write_ptr_1;
- in_busy_ptr = &smd_info->in_busy_1;
- } else if (smd_info->buf_in_2 == buf) {
- write_ptr_modem = smd_info->write_ptr_2;
- in_busy_ptr = &smd_info->in_busy_2;
- } else {
- pr_err("diag: In %s, no match for in_busy_1\n", __func__);
- }
+ /* If the data is already hdlc encoded */
+ if (!smd_info->encode_hdlc) {
+ if (smd_info->buf_in_1 == buf) {
+ write_ptr_modem = smd_info->write_ptr_1;
+ in_busy_ptr = &smd_info->in_busy_1;
+ } else if (smd_info->buf_in_2 == buf) {
+ write_ptr_modem = smd_info->write_ptr_2;
+ in_busy_ptr = &smd_info->in_busy_2;
+ } else {
+ pr_err("diag: In %s, no match for in_busy_1, peripheral: %d\n",
+ __func__, smd_info->peripheral);
+ }
- if (write_ptr_modem) {
- write_ptr_modem->length = total_recd;
- *in_busy_ptr = 1;
- err = diag_device_write(buf, smd_info->peripheral,
- write_ptr_modem);
- if (err) {
- /* Free up the buffer for future use */
- *in_busy_ptr = 0;
- pr_err_ratelimited("diag: In %s, diag_device_write error: %d\n",
- __func__, err);
+ if (write_ptr_modem) {
+ write_ptr_modem->length = total_recd;
+ *in_busy_ptr = 1;
+ err = diag_device_write(buf, smd_info->peripheral,
+ write_ptr_modem);
+ if (err) {
+ /* Free up the buffer for future use */
+ *in_busy_ptr = 0;
+ pr_err_ratelimited("diag: In %s, diag_device_write error: %d\n",
+ __func__, err);
+ }
+ }
+ } else {
+ /* The data is raw and needs to be hdlc encoded */
+ if (smd_info->buf_in_1_raw == buf) {
+ write_ptr_modem = smd_info->write_ptr_1;
+ in_busy_ptr = &smd_info->in_busy_1;
+ } else if (smd_info->buf_in_2_raw == buf) {
+ write_ptr_modem = smd_info->write_ptr_2;
+ in_busy_ptr = &smd_info->in_busy_2;
+ } else {
+ pr_err("diag: In %s, no match for in_busy_1, peripheral: %d\n",
+ __func__, smd_info->peripheral);
+ }
+
+ if (write_ptr_modem) {
+ int success = 0;
+ int write_length = 0;
+ unsigned char *write_buf = NULL;
+
+ write_length = check_bufsize_for_encoding(smd_info, buf,
+ total_recd);
+ if (write_length) {
+ write_buf = (buf == smd_info->buf_in_1_raw) ?
+ smd_info->buf_in_1 : smd_info->buf_in_2;
+ success = diag_add_hdlc_encoding(smd_info, buf,
+ total_recd, write_buf,
+ &write_length);
+ if (success) {
+ write_ptr_modem->length = write_length;
+ *in_busy_ptr = 1;
+ err = diag_device_write(write_buf,
+ smd_info->peripheral,
+ write_ptr_modem);
+ if (err) {
+ /*
+ * Free up the buffer for
+ * future use
+ */
+ *in_busy_ptr = 0;
+ pr_err_ratelimited("diag: In %s, diag_device_write error: %d\n",
+ __func__, err);
+ }
+ }
+ }
}
}
@@ -391,11 +557,32 @@
return;
}
- if (!smd_info->in_busy_1)
+ /* Determine the buffer to read the data into. */
+ if (smd_info->type == SMD_DATA_TYPE) {
+ /* If the data is raw and not hdlc encoded */
+ if (smd_info->encode_hdlc) {
+ if (!smd_info->in_busy_1)
+ buf = smd_info->buf_in_1_raw;
+ else if (!smd_info->in_busy_2)
+ buf = smd_info->buf_in_2_raw;
+ } else {
+ if (!smd_info->in_busy_1)
+ buf = smd_info->buf_in_1;
+ else if (!smd_info->in_busy_2)
+ buf = smd_info->buf_in_2;
+ }
+ } else if (smd_info->type == SMD_CMD_TYPE) {
+ /* If the data is raw and not hdlc encoded */
+ if (smd_info->encode_hdlc) {
+ if (!smd_info->in_busy_1)
+ buf = smd_info->buf_in_1_raw;
+ } else {
+ if (!smd_info->in_busy_1)
+ buf = smd_info->buf_in_1;
+ }
+ } else if (!smd_info->in_busy_1) {
buf = smd_info->buf_in_1;
- else if (!smd_info->in_busy_2 &&
- (smd_info->type == SMD_DATA_TYPE))
- buf = smd_info->buf_in_2;
+ }
if (!buf && (smd_info->type == SMD_DCI_TYPE ||
smd_info->type == SMD_DCI_CMD_TYPE))
@@ -1875,6 +2062,8 @@
kfree(smd_info->buf_in_2);
kfree(smd_info->write_ptr_1);
kfree(smd_info->write_ptr_2);
+ kfree(smd_info->buf_in_1_raw);
+ kfree(smd_info->buf_in_2_raw);
}
int diag_smd_constructor(struct diag_smd_info *smd_info, int peripheral,
@@ -1882,6 +2071,7 @@
{
smd_info->peripheral = peripheral;
smd_info->type = type;
+ smd_info->encode_hdlc = 0;
mutex_init(&smd_info->smd_ch_mutex);
switch (peripheral) {
@@ -1934,6 +2124,35 @@
goto err;
kmemleak_not_leak(smd_info->write_ptr_2);
}
+ if (driver->supports_apps_hdlc_encoding) {
+ /* In support of hdlc encoding */
+ if (smd_info->buf_in_1_raw == NULL) {
+ smd_info->buf_in_1_raw = kzalloc(IN_BUF_SIZE,
+ GFP_KERNEL);
+ if (smd_info->buf_in_1_raw == NULL)
+ goto err;
+ kmemleak_not_leak(smd_info->buf_in_1_raw);
+ }
+ if (smd_info->buf_in_2_raw == NULL) {
+ smd_info->buf_in_2_raw = kzalloc(IN_BUF_SIZE,
+ GFP_KERNEL);
+ if (smd_info->buf_in_2_raw == NULL)
+ goto err;
+ kmemleak_not_leak(smd_info->buf_in_2_raw);
+ }
+ }
+ }
+
+ if (smd_info->type == SMD_CMD_TYPE &&
+ driver->supports_apps_hdlc_encoding) {
+ /* In support of hdlc encoding */
+ if (smd_info->buf_in_1_raw == NULL) {
+ smd_info->buf_in_1_raw = kzalloc(IN_BUF_SIZE,
+ GFP_KERNEL);
+ if (smd_info->buf_in_1_raw == NULL)
+ goto err;
+ kmemleak_not_leak(smd_info->buf_in_1_raw);
+ }
}
INIT_WORK(&(smd_info->diag_read_smd_work), diag_read_smd_work_fn);
@@ -2026,6 +2245,8 @@
kfree(smd_info->buf_in_2);
kfree(smd_info->write_ptr_1);
kfree(smd_info->write_ptr_2);
+ kfree(smd_info->buf_in_1_raw);
+ kfree(smd_info->buf_in_2_raw);
return 0;
}
@@ -2048,6 +2269,7 @@
driver->buf_tbl_size = (buf_tbl_size < driver->poolsize_hdlc) ?
driver->poolsize_hdlc : buf_tbl_size;
driver->supports_separate_cmdrsp = device_supports_separate_cmdrsp();
+ driver->supports_apps_hdlc_encoding = 0;
mutex_init(&driver->diag_hdlc_mutex);
mutex_init(&driver->diag_cntl_mutex);
diff --git a/drivers/char/diag/diagfwd_cntl.c b/drivers/char/diag/diagfwd_cntl.c
index a832cb3..e0deef3 100644
--- a/drivers/char/diag/diagfwd_cntl.c
+++ b/drivers/char/diag/diagfwd_cntl.c
@@ -89,6 +89,31 @@
}
}
+static void process_hdlc_encoding_feature(struct diag_smd_info *smd_info,
+ uint8_t feature_mask)
+{
+ /*
+ * Check if apps supports hdlc encoding and the
+ * peripheral supports apps hdlc encoding
+ */
+ if (driver->supports_apps_hdlc_encoding &&
+ (feature_mask & F_DIAG_HDLC_ENCODE_IN_APPS_MASK)) {
+ driver->smd_data[smd_info->peripheral].encode_hdlc =
+ ENABLE_APPS_HDLC_ENCODING;
+ if (driver->separate_cmdrsp[smd_info->peripheral] &&
+ smd_info->peripheral < NUM_SMD_CMD_CHANNELS)
+ driver->smd_cmd[smd_info->peripheral].encode_hdlc =
+ ENABLE_APPS_HDLC_ENCODING;
+ } else {
+ driver->smd_data[smd_info->peripheral].encode_hdlc =
+ DISABLE_APPS_HDLC_ENCODING;
+ if (driver->separate_cmdrsp[smd_info->peripheral] &&
+ smd_info->peripheral < NUM_SMD_CMD_CHANNELS)
+ driver->smd_cmd[smd_info->peripheral].encode_hdlc =
+ DISABLE_APPS_HDLC_ENCODING;
+ }
+}
+
/* 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)
@@ -187,6 +212,12 @@
else
driver->separate_cmdrsp[periph] =
DISABLE_SEPARATE_CMDRSP;
+ /*
+ * Check if apps supports hdlc encoding and the
+ * peripheral supports apps hdlc encoding
+ */
+ process_hdlc_encoding_feature(smd_info,
+ feature_mask);
if (feature_mask_len > 1) {
feature_mask = *(uint8_t *)(buf+13);
process_stm_feature(smd_info,
diff --git a/drivers/char/diag/diagfwd_cntl.h b/drivers/char/diag/diagfwd_cntl.h
index c90c132..d79195c 100644
--- a/drivers/char/diag/diagfwd_cntl.h
+++ b/drivers/char/diag/diagfwd_cntl.h
@@ -48,6 +48,9 @@
/* Denotes we support diag over stm */
#define F_DIAG_OVER_STM 0x02
+ /* Perform hdlc encoding of data coming from smd channel */
+#define F_DIAG_HDLC_ENCODE_IN_APPS_MASK 0x40
+
#define ENABLE_SEPARATE_CMDRSP 1
#define DISABLE_SEPARATE_CMDRSP 0
@@ -57,6 +60,9 @@
#define UPDATE_PERIPHERAL_STM_STATE 1
#define CLEAR_PERIPHERAL_STM_STATE 2
+#define ENABLE_APPS_HDLC_ENCODING 1
+#define DISABLE_APPS_HDLC_ENCODING 0
+
struct cmd_code_range {
uint16_t cmd_code_lo;
uint16_t cmd_code_hi;