tobi/M7350
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
f75098198c
M7350/kernel/drivers/char/diag/diagfwd.c
T f75098198c M7350v5_en_gpl
2024-09-09 08:57:42 +00:00

1668 lines
45 KiB
C
Raw Blame History

/* Copyright (c) 2008-2015, The Linux Foundation. All rights reserved.
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 and
* only version 2 as published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#include <linux/slab.h>
#include <linux/init.h>
#include <linux/module.h>
#include <linux/device.h>
#include <linux/err.h>
#include <linux/sched.h>
#include <linux/ratelimit.h>
#include <linux/workqueue.h>
#include <linux/pm_runtime.h>
#include <linux/diagchar.h>
#include <linux/delay.h>
#include <linux/reboot.h>
#include <linux/of.h>
#include <linux/kmemleak.h>
#ifdef CONFIG_DIAG_OVER_USB
#include <linux/usb/usbdiag.h>
#endif
#include <soc/qcom/socinfo.h>
#include <soc/qcom/restart.h>
#include "diagmem.h"
#include "diagchar.h"
#include "diagfwd.h"
#include "diagfwd_peripheral.h"
#include "diagfwd_cntl.h"
#include "diagchar_hdlc.h"
#include "diag_dci.h"
#include "diag_masks.h"
#include "diag_usb.h"
#include "diag_mux.h"
#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_SUPPORTED_INDEX 7
#define STM_RSP_STATUS_INDEX 8
#define STM_RSP_NUM_BYTES 9
static int timestamp_switch;
module_param(timestamp_switch, int, 0644);
int wrap_enabled;
uint16_t wrap_count;
static struct diag_hdlc_decode_type *hdlc_decode;
#define DIAG_NUM_COMMON_CMD 1
static uint8_t common_cmds[DIAG_NUM_COMMON_CMD] = {
DIAG_CMD_LOG_ON_DMND
};
static uint8_t hdlc_timer_in_progress;
/* Determine if this device uses a device tree */
#ifdef CONFIG_OF
static int has_device_tree(void)
{
struct device_node *node;
node = of_find_node_by_path("/");
if (node) {
of_node_put(node);
return 1;
}
return 0;
}
#else
static int has_device_tree(void)
{
return 0;
}
#endif
int chk_config_get_id(void)
{
switch (socinfo_get_msm_cpu()) {
case MSM_CPU_8X60:
return APQ8060_TOOLS_ID;
case MSM_CPU_8960:
case MSM_CPU_8960AB:
return AO8960_TOOLS_ID;
case MSM_CPU_8064:
case MSM_CPU_8064AB:
case MSM_CPU_8064AA:
return APQ8064_TOOLS_ID;
case MSM_CPU_8930:
case MSM_CPU_8930AA:
case MSM_CPU_8930AB:
return MSM8930_TOOLS_ID;
case MSM_CPU_8974:
return MSM8974_TOOLS_ID;
case MSM_CPU_8625:
return MSM8625_TOOLS_ID;
case MSM_CPU_8084:
return APQ8084_TOOLS_ID;
case MSM_CPU_8916:
return MSM8916_TOOLS_ID;
case MSM_CPU_8939:
return MSM8939_TOOLS_ID;
case MSM_CPU_8994:
return MSM8994_TOOLS_ID;
case MSM_CPU_8226:
return APQ8026_TOOLS_ID;
case MSM_CPU_8909:
return MSM8909_TOOLS_ID;
case MSM_CPU_8992:
return MSM8992_TOOLS_ID;
case MSM_CPU_8996:
return MSM_8996_TOOLS_ID;
case MSM_CPU_8952:
return MSM8952_TOOLS_ID;
default:
if (driver->use_device_tree) {
if (machine_is_msm8974())
return MSM8974_TOOLS_ID;
else if (machine_is_apq8074())
return APQ8074_TOOLS_ID;
else
return 0;
} else {
return 0;
}
}
}
/*
* This will return TRUE for targets which support apps only mode and hence SSR.
* This applies to 8960 and newer targets.
*/
int chk_apps_only(void)
{
if (driver->use_device_tree)
return 1;
switch (socinfo_get_msm_cpu()) {
case MSM_CPU_8960:
case MSM_CPU_8960AB:
case MSM_CPU_8064:
case MSM_CPU_8064AB:
case MSM_CPU_8064AA:
case MSM_CPU_8930:
case MSM_CPU_8930AA:
case MSM_CPU_8930AB:
case MSM_CPU_8627:
case MSM_CPU_9615:
case MSM_CPU_8974:
return 1;
default:
return 0;
}
}
/*
* This will return TRUE for targets which support apps as master.
* Thus, SW DLOAD and Mode Reset are supported on apps processor.
* This applies to 8960 and newer targets.
*/
int chk_apps_master(void)
{
if (driver->use_device_tree)
return 1;
else
return 0;
}
int chk_polling_response(void)
{
if (!(driver->polling_reg_flag) && chk_apps_master())
/*
* If the apps processor is master and no other processor
* has registered to respond for polling
*/
return 1;
else if (!(driver->diagfwd_cntl[PERIPHERAL_MODEM] &&
driver->diagfwd_cntl[PERIPHERAL_MODEM]->ch_open) &&
(driver->feature[PERIPHERAL_MODEM].rcvd_feature_mask))
/*
* If the apps processor is not the master and the modem
* is not up or we did not receive the feature masks from Modem
*/
return 1;
else
return 0;
}
/*
* This function should be called if you feel that the logging process may
* need to be woken up. For instance, if the logging mode is MEMORY_DEVICE MODE
* and while trying to read data from data channel there are no buffers
* available to read the data into, then this function should be called to
* determine if the logging process needs to be woken up.
*/
void chk_logging_wakeup(void)
{
int i;
int j;
int pid = 0;
for (j = 0; j < NUM_MD_SESSIONS; j++) {
if (!driver->md_session_map[j])
continue;
pid = driver->md_session_map[j]->pid;
/* Find the index of the logging process */
for (i = 0; i < driver->num_clients; i++) {
if (driver->client_map[i].pid != pid)
continue;
if (driver->data_ready[i] & USER_SPACE_DATA_TYPE)
continue;
/*
* At very high logging rates a race condition can
* occur where the buffers containing the data from
* a channel are all in use, but the data_ready flag
* is cleared. In this case, the buffers never have
* their data read/logged. Detect and remedy this
* situation.
*/
driver->data_ready[i] |= USER_SPACE_DATA_TYPE;
pr_debug("diag: Force wakeup of logging process\n");
wake_up_interruptible(&driver->wait_q);
break;
}
/*
* Diag Memory Device is in normal. Check only for the first
* index as all the indices point to the same session
* structure.
*/
if (driver->md_session_mode == DIAG_MD_NORMAL && j == 0)
break;
}
}
static void pack_rsp_and_send(unsigned char *buf, int len)
{
int err;
int retry_count = 0;
uint32_t write_len = 0;
unsigned long flags;
unsigned char *rsp_ptr = driver->encoded_rsp_buf;
struct diag_pkt_frame_t header;
if (!rsp_ptr || !buf)
return;
if (len > DIAG_MAX_RSP_SIZE || len < 0) {
pr_err("diag: In %s, invalid len %d, permissible len %d\n",
__func__, len, DIAG_MAX_RSP_SIZE);
return;
}
/*
* 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
* response.
*/
while (retry_count < UINT_MAX) {
if (!driver->rsp_buf_busy)
break;
/*
* Wait for sometime and try again. The value 10000 was chosen
* empirically as an optimum value for USB to complete a write
*/
usleep_range(10000, 10100);
retry_count++;
/*
* There can be a race conditon that clears the data ready flag
* for responses. Make sure we don't miss previous wakeups for
* draining responses when we are in Memory Device Mode.
*/
if (driver->logging_mode == DIAG_MEMORY_DEVICE_MODE)
chk_logging_wakeup();
}
if (driver->rsp_buf_busy) {
pr_err("diag: unable to get hold of response buffer\n");
return;
}
driver->rsp_buf_busy = 1;
header.start = CONTROL_CHAR;
header.version = 1;
header.length = len;
memcpy(rsp_ptr, &header, sizeof(header));
write_len += sizeof(header);
memcpy(rsp_ptr + write_len, buf, len);
write_len += 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);
if (err) {
pr_err("diag: In %s, unable to write to mux, err: %d\n",
__func__, err);
spin_lock_irqsave(&driver->rsp_buf_busy_lock, flags);
driver->rsp_buf_busy = 0;
spin_unlock_irqrestore(&driver->rsp_buf_busy_lock, flags);
}
}
static void encode_rsp_and_send(unsigned char *buf, int len)
{
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;
unsigned long flags;
if (!rsp_ptr || !buf)
return;
if (len > DIAG_MAX_RSP_SIZE || len < 0) {
pr_err("diag: In %s, invalid len %d, permissible len %d\n",
__func__, len, DIAG_MAX_RSP_SIZE);
return;
}
/*
* 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
* response.
*/
while (retry_count < UINT_MAX) {
if (!driver->rsp_buf_busy)
break;
/*
* Wait for sometime and try again. The value 10000 was chosen
* empirically as an optimum value for USB to complete a write
*/
usleep_range(10000, 10100);
retry_count++;
/*
* There can be a race conditon that clears the data ready flag
* for responses. Make sure we don't miss previous wakeups for
* draining responses when we are in Memory Device Mode.
*/
if (driver->logging_mode == DIAG_MEMORY_DEVICE_MODE)
chk_logging_wakeup();
}
if (driver->rsp_buf_busy) {
pr_err("diag: unable to get hold of response buffer\n");
return;
}
spin_lock_irqsave(&driver->rsp_buf_busy_lock, flags);
driver->rsp_buf_busy = 1;
spin_unlock_irqrestore(&driver->rsp_buf_busy_lock, flags);
send.state = DIAG_STATE_START;
send.pkt = buf;
send.last = (void *)(buf + len - 1);
send.terminate = 1;
enc.dest = rsp_ptr;
enc.dest_last = (void *)(rsp_ptr + DIAG_MAX_HDLC_BUF_SIZE - 1);
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);
if (err) {
pr_err("diag: In %s, Unable to write to device, err: %d\n",
__func__, err);
spin_lock_irqsave(&driver->rsp_buf_busy_lock, flags);
driver->rsp_buf_busy = 0;
spin_unlock_irqrestore(&driver->rsp_buf_busy_lock, flags);
}
memset(buf, '\0', DIAG_MAX_RSP_SIZE);
}
void diag_send_rsp(unsigned char *buf, int len)
{
struct diag_md_session_t *session_info = NULL;
uint8_t hdlc_disabled;
session_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);
else
encode_rsp_and_send(buf, len);
}
void diag_update_pkt_buffer(unsigned char *buf, uint32_t len, int type)
{
unsigned char *ptr = NULL;
unsigned char *temp = buf;
int *in_busy = NULL;
uint32_t *length = NULL;
uint32_t max_len = 0;
if (!buf || len == 0) {
pr_err("diag: In %s, Invalid ptr %p and length %d\n",
__func__, buf, len);
return;
}
switch (type) {
case PKT_TYPE:
ptr = driver->apps_req_buf;
length = &driver->apps_req_buf_len;
max_len = DIAG_MAX_REQ_SIZE;
in_busy = &driver->in_busy_pktdata;
break;
case DCI_PKT_TYPE:
ptr = driver->dci_pkt_buf;
length = &driver->dci_pkt_length;
max_len = DCI_BUF_SIZE;
in_busy = &driver->in_busy_dcipktdata;
break;
default:
pr_err("diag: Invalid type %d in %s\n", type, __func__);
return;
}
mutex_lock(&driver->diagchar_mutex);
if (CHK_OVERFLOW(ptr, ptr, ptr + max_len, len)) {
memcpy(ptr, temp , len);
*length = len;
*in_busy = 1;
} else {
pr_alert("diag: In %s, no space for response packet, len: %d, type: %d\n",
__func__, len, type);
}
mutex_unlock(&driver->diagchar_mutex);
}
void diag_update_userspace_clients(unsigned int type)
{
int i;
mutex_lock(&driver->diagchar_mutex);
for (i = 0; i < driver->num_clients; i++)
if (driver->client_map[i].pid != 0)
driver->data_ready[i] |= type;
wake_up_interruptible(&driver->wait_q);
mutex_unlock(&driver->diagchar_mutex);
}
void diag_update_md_clients(unsigned int type)
{
int i, j;
mutex_lock(&driver->diagchar_mutex);
for (i = 0; i < NUM_MD_SESSIONS; i++) {
if (driver->md_session_map[i] != NULL)
for (j = 0; j < driver->num_clients; j++) {
if (driver->client_map[j].pid != 0 &&
driver->client_map[j].pid ==
driver->md_session_map[i]->pid) {
driver->data_ready[j] |= type;
break;
}
}
}
wake_up_interruptible(&driver->wait_q);
mutex_unlock(&driver->diagchar_mutex);
}
void diag_update_sleeping_process(int process_id, int data_type)
{
int i;
mutex_lock(&driver->diagchar_mutex);
for (i = 0; i < driver->num_clients; i++)
if (driver->client_map[i].pid == process_id) {
driver->data_ready[i] |= data_type;
break;
}
wake_up_interruptible(&driver->wait_q);
mutex_unlock(&driver->diagchar_mutex);
}
static int diag_send_data(struct diag_cmd_reg_t *entry, unsigned char *buf,
int len)
{
if (!entry)
return -EIO;
if (entry->proc == APPS_DATA) {
diag_update_pkt_buffer(buf, len, PKT_TYPE);
diag_update_sleeping_process(entry->pid, PKT_TYPE);
return 0;
}
return diagfwd_write(entry->proc, TYPE_CMD, buf, len);
}
void diag_process_stm_mask(uint8_t cmd, uint8_t data_mask, int data_type)
{
int status = 0;
if (data_type >= PERIPHERAL_MODEM && data_type <= PERIPHERAL_SENSORS) {
if (driver->feature[data_type].stm_support) {
status = diag_send_stm_state(data_type, cmd);
if (status == 0)
driver->stm_state[data_type] = cmd;
}
driver->stm_state_requested[data_type] = cmd;
} else if (data_type == APPS_DATA) {
driver->stm_state[data_type] = cmd;
driver->stm_state_requested[data_type] = cmd;
}
}
int diag_process_stm_cmd(unsigned char *buf, unsigned char *dest_buf)
{
uint8_t version, mask, cmd;
uint8_t rsp_supported = 0;
uint8_t rsp_status = 0;
int i;
if (!buf || !dest_buf) {
pr_err("diag: Invalid pointers buf: %p, dest_buf %p in %s\n",
buf, dest_buf, __func__);
return -EIO;
}
version = *(buf + STM_CMD_VERSION_OFFSET);
mask = *(buf + STM_CMD_MASK_OFFSET);
cmd = *(buf + STM_CMD_DATA_OFFSET);
/*
* Check if command is valid. If the command is asking for
* status, then the processor mask field is to be ignored.
*/
if ((version != 2) || (cmd > STATUS_STM) ||
((cmd != STATUS_STM) && ((mask == 0) || (0 != (mask >> 4))))) {
/* Command is invalid. Send bad param message response */
dest_buf[0] = BAD_PARAM_RESPONSE_MESSAGE;
for (i = 0; i < STM_CMD_NUM_BYTES; i++)
dest_buf[i+1] = *(buf + i);
return STM_CMD_NUM_BYTES+1;
} else if (cmd != STATUS_STM) {
if (mask & DIAG_STM_MODEM)
diag_process_stm_mask(cmd, DIAG_STM_MODEM,
PERIPHERAL_MODEM);
if (mask & DIAG_STM_LPASS)
diag_process_stm_mask(cmd, DIAG_STM_LPASS,
PERIPHERAL_LPASS);
if (mask & DIAG_STM_WCNSS)
diag_process_stm_mask(cmd, DIAG_STM_WCNSS,
PERIPHERAL_WCNSS);
if (mask & DIAG_STM_SENSORS)
diag_process_stm_mask(cmd, DIAG_STM_SENSORS,
PERIPHERAL_SENSORS);
if (mask & DIAG_STM_APPS)
diag_process_stm_mask(cmd, DIAG_STM_APPS, APPS_DATA);
}
for (i = 0; i < STM_CMD_NUM_BYTES; i++)
dest_buf[i] = *(buf + i);
/* Set mask denoting which peripherals support STM */
if (driver->feature[PERIPHERAL_MODEM].stm_support)
rsp_supported |= DIAG_STM_MODEM;
if (driver->feature[PERIPHERAL_LPASS].stm_support)
rsp_supported |= DIAG_STM_LPASS;
if (driver->feature[PERIPHERAL_WCNSS].stm_support)
rsp_supported |= DIAG_STM_WCNSS;
if (driver->feature[PERIPHERAL_SENSORS].stm_support)
rsp_supported |= DIAG_STM_SENSORS;
rsp_supported |= DIAG_STM_APPS;
/* Set mask denoting STM state/status for each peripheral/APSS */
if (driver->stm_state[PERIPHERAL_MODEM])
rsp_status |= DIAG_STM_MODEM;
if (driver->stm_state[PERIPHERAL_LPASS])
rsp_status |= DIAG_STM_LPASS;
if (driver->stm_state[PERIPHERAL_WCNSS])
rsp_status |= DIAG_STM_WCNSS;
if (driver->stm_state[PERIPHERAL_SENSORS])
rsp_status |= DIAG_STM_SENSORS;
if (driver->stm_state[APPS_DATA])
rsp_status |= DIAG_STM_APPS;
dest_buf[STM_RSP_SUPPORTED_INDEX] = rsp_supported;
dest_buf[STM_RSP_STATUS_INDEX] = rsp_status;
return STM_RSP_NUM_BYTES;
}
int diag_process_time_sync_query_cmd(unsigned char *src_buf, int src_len,
unsigned char *dest_buf, int dest_len)
{
int write_len = 0;
struct diag_cmd_time_sync_query_req_t *req = NULL;
struct diag_cmd_time_sync_query_rsp_t rsp;
if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0) {
pr_err("diag: Invalid input in %s, src_buf: %p, src_len: %d, dest_buf: %p, dest_len: %d",
__func__, src_buf, src_len, dest_buf, dest_len);
return -EINVAL;
}
req = (struct diag_cmd_time_sync_query_req_t *)src_buf;
rsp.header.cmd_code = req->header.cmd_code;
rsp.header.subsys_id = req->header.subsys_id;
rsp.header.subsys_cmd_code = req->header.subsys_cmd_code;
rsp.version = req->version;
rsp.time_api = driver->uses_time_api;
memcpy(dest_buf, &rsp, sizeof(rsp));
write_len = sizeof(rsp);
return write_len;
}
int diag_process_time_sync_switch_cmd(unsigned char *src_buf, int src_len,
unsigned char *dest_buf, int dest_len)
{
uint8_t peripheral, status = 0;
struct diag_cmd_time_sync_switch_req_t *req = NULL;
struct diag_cmd_time_sync_switch_rsp_t rsp;
struct diag_ctrl_msg_time_sync time_sync_msg;
int msg_size = sizeof(struct diag_ctrl_msg_time_sync);
int err = 0, write_len = 0;
if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0) {
pr_err("diag: Invalid input in %s, src_buf: %p, src_len: %d, dest_buf: %p, dest_len: %d",
__func__, src_buf, src_len, dest_buf, dest_len);
return -EINVAL;
}
req = (struct diag_cmd_time_sync_switch_req_t *)src_buf;
rsp.header.cmd_code = req->header.cmd_code;
rsp.header.subsys_id = req->header.subsys_id;
rsp.header.subsys_cmd_code = req->header.subsys_cmd_code;
rsp.version = req->version;
rsp.time_api = req->time_api;
if ((req->version > 1) || (req->time_api > 1) ||
(req->persist_time > 0)) {
dest_buf[0] = BAD_PARAM_RESPONSE_MESSAGE;
rsp.time_api_status = 0;
rsp.persist_time_status = PERSIST_TIME_NOT_SUPPORTED;
memcpy(dest_buf + 1, &rsp, sizeof(rsp));
write_len = sizeof(rsp) + 1;
timestamp_switch = 0;
return write_len;
}
time_sync_msg.ctrl_pkt_id = DIAG_CTRL_MSG_TIME_SYNC_PKT;
time_sync_msg.ctrl_pkt_data_len = 5;
time_sync_msg.version = 1;
time_sync_msg.time_api = req->time_api;
for (peripheral = 0; peripheral < NUM_PERIPHERALS; peripheral++) {
err = diagfwd_write(peripheral, TYPE_CNTL, &time_sync_msg,
msg_size);
if (err && err != -ENODEV) {
pr_err("diag: In %s, unable to write to peripheral: %d, type: %d, len: %d, err: %d\n",
__func__, peripheral, TYPE_CNTL,
msg_size, err);
status |= (1 << peripheral);
}
}
driver->time_sync_enabled = 1;
driver->uses_time_api = req->time_api;
switch (req->time_api) {
case 0:
timestamp_switch = 0;
break;
case 1:
timestamp_switch = 1;
break;
default:
timestamp_switch = 0;
break;
}
rsp.time_api_status = status;
rsp.persist_time_status = PERSIST_TIME_NOT_SUPPORTED;
memcpy(dest_buf, &rsp, sizeof(rsp));
write_len = sizeof(rsp);
return write_len;
}
int diag_cmd_log_on_demand(unsigned char *src_buf, int src_len,
unsigned char *dest_buf, int dest_len)
{
int write_len = 0;
struct diag_log_on_demand_rsp_t header;
if (!driver->diagfwd_cntl[PERIPHERAL_MODEM] ||
!driver->diagfwd_cntl[PERIPHERAL_MODEM]->ch_open ||
!driver->log_on_demand_support)
return 0;
if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0) {
pr_err("diag: Invalid input in %s, src_buf: %p, src_len: %d, dest_buf: %p, dest_len: %d",
__func__, src_buf, src_len, dest_buf, dest_len);
return -EINVAL;
}
header.cmd_code = DIAG_CMD_LOG_ON_DMND;
header.log_code = *(uint16_t *)(src_buf + 1);
header.status = 1;
memcpy(dest_buf, &header, sizeof(struct diag_log_on_demand_rsp_t));
write_len += sizeof(struct diag_log_on_demand_rsp_t);
return write_len;
}
int diag_cmd_get_mobile_id(unsigned char *src_buf, int src_len,
unsigned char *dest_buf, int dest_len)
{
int write_len = 0;
struct diag_pkt_header_t *header = NULL;
struct diag_cmd_ext_mobile_rsp_t rsp;
if (!src_buf || src_len != sizeof(*header) || !dest_buf ||
dest_len < sizeof(rsp))
return -EIO;
header = (struct diag_pkt_header_t *)src_buf;
rsp.header.cmd_code = header->cmd_code;
rsp.header.subsys_id = header->subsys_id;
rsp.header.subsys_cmd_code = header->subsys_cmd_code;
rsp.version = 2;
rsp.padding[0] = 0;
rsp.padding[1] = 0;
rsp.padding[2] = 0;
rsp.family = 0;
rsp.chip_id = (uint32_t)socinfo_get_id();
memcpy(dest_buf, &rsp, sizeof(rsp));
write_len += sizeof(rsp);
return write_len;
}
int diag_check_common_cmd(struct diag_pkt_header_t *header)
{
int i;
if (!header)
return -EIO;
for (i = 0; i < DIAG_NUM_COMMON_CMD; i++) {
if (header->cmd_code == common_cmds[i])
return 1;
}
return 0;
}
static int diag_cmd_chk_stats(unsigned char *src_buf, int src_len,
unsigned char *dest_buf, int dest_len)
{
int payload = 0;
int write_len = 0;
struct diag_pkt_header_t *header = NULL;
struct diag_cmd_stats_rsp_t rsp;
if (!src_buf || src_len < sizeof(struct diag_pkt_header_t) ||
!dest_buf || dest_len < sizeof(rsp))
return -EINVAL;
header = (struct diag_pkt_header_t *)src_buf;
if (header->cmd_code != DIAG_CMD_DIAG_SUBSYS ||
header->subsys_id != DIAG_SS_DIAG)
return -EINVAL;
switch (header->subsys_cmd_code) {
case DIAG_CMD_OP_GET_MSG_ALLOC:
payload = driver->msg_stats.alloc_count;
break;
case DIAG_CMD_OP_GET_MSG_DROP:
payload = driver->msg_stats.drop_count;
break;
case DIAG_CMD_OP_RESET_MSG_STATS:
diag_record_stats(DATA_TYPE_F3, PKT_RESET);
break;
case DIAG_CMD_OP_GET_LOG_ALLOC:
payload = driver->log_stats.alloc_count;
break;
case DIAG_CMD_OP_GET_LOG_DROP:
payload = driver->log_stats.drop_count;
break;
case DIAG_CMD_OP_RESET_LOG_STATS:
diag_record_stats(DATA_TYPE_LOG, PKT_RESET);
break;
case DIAG_CMD_OP_GET_EVENT_ALLOC:
payload = driver->event_stats.alloc_count;
break;
case DIAG_CMD_OP_GET_EVENT_DROP:
payload = driver->event_stats.drop_count;
break;
case DIAG_CMD_OP_RESET_EVENT_STATS:
diag_record_stats(DATA_TYPE_EVENT, PKT_RESET);
break;
default:
return -EINVAL;
}
memcpy(&rsp.header, header, sizeof(struct diag_pkt_header_t));
rsp.payload = payload;
write_len = sizeof(rsp);
memcpy(dest_buf, &rsp, sizeof(rsp));
return write_len;
}
static int diag_cmd_disable_hdlc(unsigned char *src_buf, int src_len,
unsigned char *dest_buf, int dest_len)
{
struct diag_pkt_header_t *header = NULL;
struct diag_cmd_hdlc_disable_rsp_t rsp;
int write_len = 0;
if (!src_buf || src_len < sizeof(*header) ||
!dest_buf || dest_len < sizeof(rsp)) {
return -EIO;
}
header = (struct diag_pkt_header_t *)src_buf;
if (header->cmd_code != DIAG_CMD_DIAG_SUBSYS ||
header->subsys_id != DIAG_SS_DIAG ||
header->subsys_cmd_code != DIAG_CMD_OP_HDLC_DISABLE) {
return -EINVAL;
}
memcpy(&rsp.header, header, sizeof(struct diag_pkt_header_t));
rsp.framing_version = 1;
rsp.result = 0;
write_len = sizeof(rsp);
memcpy(dest_buf, &rsp, sizeof(rsp));
return write_len;
}
void diag_send_error_rsp(unsigned char *buf, int len)
{
/* -1 to accomodate the first byte 0x13 */
if (len > (DIAG_MAX_RSP_SIZE - 1)) {
pr_err("diag: cannot send err rsp, huge length: %d\n", len);
return;
}
*(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);
}
int diag_process_apps_pkt(unsigned char *buf, int len,
struct diag_md_session_t *info)
{
int i;
int mask_ret;
int write_len = 0;
unsigned char *temp = NULL;
struct diag_cmd_reg_entry_t entry;
struct diag_cmd_reg_entry_t *temp_entry = NULL;
struct diag_cmd_reg_t *reg_item = NULL;
if (!buf)
return -EIO;
/* 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);
return 0;
}
temp = buf;
entry.cmd_code = (uint16_t)(*(uint8_t *)temp);
temp += sizeof(uint8_t);
entry.subsys_id = (uint16_t)(*(uint8_t *)temp);
temp += sizeof(uint8_t);
entry.cmd_code_hi = (uint16_t)(*(uint16_t *)temp);
entry.cmd_code_lo = (uint16_t)(*(uint16_t *)temp);
temp += sizeof(uint16_t);
pr_debug("diag: In %s, received cmd %02x %02x %02x\n",
__func__, entry.cmd_code, entry.subsys_id, entry.cmd_code_hi);
if (*buf == DIAG_CMD_LOG_ON_DMND && driver->log_on_demand_support &&
driver->feature[PERIPHERAL_MODEM].rcvd_feature_mask) {
write_len = diag_cmd_log_on_demand(buf, len,
driver->apps_rsp_buf,
DIAG_MAX_RSP_SIZE);
if (write_len > 0)
diag_send_rsp(driver->apps_rsp_buf, write_len);
return 0;
}
mutex_lock(&driver->cmd_reg_mutex);
temp_entry = diag_cmd_search(&entry, ALL_PROC);
if (temp_entry) {
reg_item = container_of(temp_entry, struct diag_cmd_reg_t,
entry);
if (info) {
if (MD_PERIPHERAL_MASK(reg_item->proc) &
info->peripheral_mask)
write_len = diag_send_data(reg_item, buf, len);
} else
write_len = diag_send_data(reg_item, buf, len);
mutex_unlock(&driver->cmd_reg_mutex);
return write_len;
}
mutex_unlock(&driver->cmd_reg_mutex);
#if defined(CONFIG_DIAG_OVER_USB)
/* Check for the command/respond msg for the maximum packet length */
if ((*buf == 0x4b) && (*(buf+1) == 0x12) &&
(*(uint16_t *)(buf+2) == 0x0055)) {
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);
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);
return 0;
}
return len;
}
/* Check for time sync query command */
else if ((*buf == DIAG_CMD_DIAG_SUBSYS) &&
(*(buf+1) == DIAG_SS_DIAG) &&
(*(uint16_t *)(buf+2) == DIAG_GET_TIME_API)) {
write_len = diag_process_time_sync_query_cmd(buf, len,
driver->apps_rsp_buf,
DIAG_MAX_RSP_SIZE);
if (write_len > 0)
diag_send_rsp(driver->apps_rsp_buf, write_len);
return 0;
}
/* Check for time sync switch command */
else if ((*buf == DIAG_CMD_DIAG_SUBSYS) &&
(*(buf+1) == DIAG_SS_DIAG) &&
(*(uint16_t *)(buf+2) == DIAG_SET_TIME_API)) {
write_len = diag_process_time_sync_switch_cmd(buf, len,
driver->apps_rsp_buf,
DIAG_MAX_RSP_SIZE);
if (write_len > 0)
diag_send_rsp(driver->apps_rsp_buf, write_len);
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);
msleep(5000);
/* call download API */
msm_set_restart_mode(RESTART_DLOAD);
printk(KERN_CRIT "diag: download mode set, Rebooting SoC..\n");
kernel_restart(NULL);
/* Not required, represents that command isnt sent to modem */
return 0;
}
/* Check for polling for Apps only DIAG */
else if ((*buf == 0x4b) && (*(buf+1) == 0x32) &&
(*(buf+2) == 0x03)) {
/* If no one has registered for polling */
if (chk_polling_response()) {
/* Respond to polling for Apps only DIAG */
for (i = 0; i < 3; i++)
driver->apps_rsp_buf[i] = *(buf+i);
for (i = 0; i < 13; i++)
driver->apps_rsp_buf[i+3] = 0;
diag_send_rsp(driver->apps_rsp_buf, 16);
return 0;
}
}
/* Return the Delayed Response Wrap Status */
else if ((*buf == 0x4b) && (*(buf+1) == 0x32) &&
(*(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);
return 0;
}
/* Wrap the Delayed Rsp ID */
else if ((*buf == 0x4b) && (*(buf+1) == 0x32) &&
(*(buf+2) == 0x05) && (*(buf+3) == 0x0)) {
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);
return 0;
}
/* Mobile ID Rsp */
else if ((*buf == DIAG_CMD_DIAG_SUBSYS) &&
(*(buf+1) == DIAG_SS_PARAMS) &&
(*(buf+2) == DIAG_EXT_MOBILE_ID) && (*(buf+3) == 0x0)) {
write_len = diag_cmd_get_mobile_id(buf, len,
driver->apps_rsp_buf,
DIAG_MAX_RSP_SIZE);
if (write_len > 0) {
diag_send_rsp(driver->apps_rsp_buf, write_len);
return 0;
}
}
/*
* If the apps processor is master and no other
* processor has registered for polling command.
* If modem is not up and we have not received feature
* mask update from modem, in that case APPS should
* respond for 0X7C command
*/
else if (chk_apps_master() &&
!(driver->polling_reg_flag) &&
!(driver->diagfwd_cntl[PERIPHERAL_MODEM]->ch_open) &&
!(driver->feature[PERIPHERAL_MODEM].rcvd_feature_mask)) {
/* respond to 0x0 command */
if (*buf == 0x00) {
for (i = 0; i < 55; i++)
driver->apps_rsp_buf[i] = 0;
diag_send_rsp(driver->apps_rsp_buf, 55);
return 0;
}
/* respond to 0x7c command */
else if (*buf == 0x7c) {
driver->apps_rsp_buf[0] = 0x7c;
for (i = 1; i < 8; i++)
driver->apps_rsp_buf[i] = 0;
/* Tools ID for APQ 8060 */
*(int *)(driver->apps_rsp_buf + 8) =
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);
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);
return 0;
}
write_len = diag_cmd_disable_hdlc(buf, len, driver->apps_rsp_buf,
DIAG_MAX_RSP_SIZE);
if (write_len > 0) {
/*
* This mutex lock is necessary since we need to drain all the
* pending buffers from peripherals which may be HDLC encoded
* before disabling HDLC encoding on Apps processor.
*/
mutex_lock(&driver->hdlc_disable_mutex);
diag_send_rsp(driver->apps_rsp_buf, write_len);
/*
* Set the value of hdlc_disabled after sending the response to
* the tools. This is required since the tools is expecting a
* HDLC encoded reponse for this request.
*/
pr_debug("diag: In %s, disabling HDLC encoding\n",
__func__);
if (info)
info->hdlc_disabled = 1;
else
driver->hdlc_disabled = 1;
diag_update_md_clients(HDLC_SUPPORT_TYPE);
mutex_unlock(&driver->hdlc_disable_mutex);
return 0;
}
#endif
/* We have now come to the end of the function. */
if (chk_apps_only())
diag_send_error_rsp(buf, len);
return 0;
}
void diag_process_hdlc_pkt(void *data, unsigned len,
struct diag_md_session_t *info)
{
int err = 0;
int ret = 0;
if (len > DIAG_MAX_HDLC_BUF_SIZE) {
pr_err("diag: In %s, invalid length: %d\n", __func__, len);
return;
}
mutex_lock(&driver->diag_hdlc_mutex);
pr_debug("diag: In %s, received packet of length: %d, req_buf_len: %d\n",
__func__, len, driver->hdlc_buf_len);
if (driver->hdlc_buf_len >= DIAG_MAX_REQ_SIZE) {
pr_err("diag: In %s, request length is more than supported len. Dropping packet.\n",
__func__);
goto fail;
}
hdlc_decode->dest_ptr = driver->hdlc_buf + driver->hdlc_buf_len;
hdlc_decode->dest_size = DIAG_MAX_HDLC_BUF_SIZE - driver->hdlc_buf_len;
hdlc_decode->src_ptr = data;
hdlc_decode->src_size = len;
hdlc_decode->src_idx = 0;
hdlc_decode->dest_idx = 0;
ret = diag_hdlc_decode(hdlc_decode);
/*
* driver->hdlc_buf is of size DIAG_MAX_HDLC_BUF_SIZE. But the decoded
* packet should be within DIAG_MAX_REQ_SIZE.
*/
if (driver->hdlc_buf_len + hdlc_decode->dest_idx <= DIAG_MAX_REQ_SIZE) {
driver->hdlc_buf_len += hdlc_decode->dest_idx;
} else {
pr_err_ratelimited("diag: In %s, Dropping packet. pkt_size: %d, max: %d\n",
__func__,
driver->hdlc_buf_len + hdlc_decode->dest_idx,
DIAG_MAX_REQ_SIZE);
goto fail;
}
if (ret == HDLC_COMPLETE) {
err = crc_check(driver->hdlc_buf, driver->hdlc_buf_len);
if (err) {
/* CRC check failed. */
pr_err_ratelimited("diag: In %s, bad CRC. Dropping packet\n",
__func__);
goto fail;
}
driver->hdlc_buf_len -= HDLC_FOOTER_LEN;
if (driver->hdlc_buf_len < 1) {
pr_err_ratelimited("diag: In %s, message is too short, len: %d, dest len: %d\n",
__func__, driver->hdlc_buf_len,
hdlc_decode->dest_idx);
goto fail;
}
err = diag_process_apps_pkt(driver->hdlc_buf,
driver->hdlc_buf_len, info);
if (err < 0)
goto fail;
} else {
goto end;
}
driver->hdlc_buf_len = 0;
mutex_unlock(&driver->diag_hdlc_mutex);
return;
fail:
/*
* Tools needs to get a response in order to start its
* 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);
driver->hdlc_buf_len = 0;
end:
mutex_unlock(&driver->diag_hdlc_mutex);
}
static int diagfwd_mux_open(int id, int mode)
{
uint8_t i;
unsigned long flags;
switch (mode) {
case DIAG_USB_MODE:
driver->usb_connected = 1;
break;
case DIAG_MEMORY_DEVICE_MODE:
break;
default:
return -EINVAL;
}
if (driver->rsp_buf_busy) {
/*
* When a client switches from callback mode to USB mode
* explicitly, there can be a situation when the last response
* is not drained to the user space application. Reset the
* in_busy flag in this case.
*/
spin_lock_irqsave(&driver->rsp_buf_busy_lock, flags);
driver->rsp_buf_busy = 0;
spin_unlock_irqrestore(&driver->rsp_buf_busy_lock, flags);
}
for (i = 0; i < NUM_PERIPHERALS; i++) {
diagfwd_open(i, TYPE_DATA);
diagfwd_open(i, TYPE_CMD);
}
queue_work(driver->diag_real_time_wq, &driver->diag_real_time_work);
return 0;
}
static int diagfwd_mux_close(int id, int mode)
{
uint8_t i;
switch (mode) {
case DIAG_USB_MODE:
driver->usb_connected = 0;
break;
case DIAG_MEMORY_DEVICE_MODE:
break;
default:
return -EINVAL;
}
if ((mode == DIAG_USB_MODE &&
driver->logging_mode == DIAG_MEMORY_DEVICE_MODE) ||
(mode == DIAG_MEMORY_DEVICE_MODE &&
driver->logging_mode == DIAG_USB_MODE)) {
/*
* In this case the channel must not be closed. This case
* indicates that the USB is removed but there is a client
* running in background with Memory Device mode
*/
} else {
for (i = 0; i < NUM_PERIPHERALS; i++) {
diagfwd_close(i, TYPE_DATA);
diagfwd_close(i, TYPE_CMD);
}
/* Re enable HDLC encoding */
pr_debug("diag: In %s, re-enabling HDLC encoding\n",
__func__);
mutex_lock(&driver->hdlc_disable_mutex);
if (driver->md_session_mode == DIAG_MD_NONE)
driver->hdlc_disabled = 0;
mutex_unlock(&driver->hdlc_disable_mutex);
queue_work(driver->diag_wq,
&(driver->update_user_clients));
}
queue_work(driver->diag_real_time_wq,
&driver->diag_real_time_work);
return 0;
}
static uint8_t hdlc_reset;
static void hdlc_reset_timer_start(struct diag_md_session_t *info)
{
if (!hdlc_timer_in_progress) {
hdlc_timer_in_progress = 1;
if (info)
mod_timer(&info->hdlc_reset_timer,
jiffies + msecs_to_jiffies(200));
else
mod_timer(&driver->hdlc_reset_timer,
jiffies + msecs_to_jiffies(200));
}
}
static void hdlc_reset_timer_func(unsigned long data)
{
pr_debug("diag: In %s, re-enabling HDLC encoding\n",
__func__);
if (hdlc_reset) {
driver->hdlc_disabled = 0;
queue_work(driver->diag_wq,
&(driver->update_user_clients));
}
hdlc_timer_in_progress = 0;
}
void diag_md_hdlc_reset_timer_func(unsigned long pid)
{
struct diag_md_session_t *session_info = NULL;
pr_debug("diag: In %s, re-enabling HDLC encoding\n",
__func__);
if (hdlc_reset) {
session_info = diag_md_session_get_pid(pid);
if (session_info)
session_info->hdlc_disabled = 0;
queue_work(driver->diag_wq,
&(driver->update_md_clients));
}
hdlc_timer_in_progress = 0;
}
static void diag_hdlc_start_recovery(unsigned char *buf, int len,
struct diag_md_session_t *info)
{
int i;
static uint32_t bad_byte_counter;
unsigned char *start_ptr = NULL;
struct diag_pkt_frame_t *actual_pkt = NULL;
hdlc_reset = 1;
hdlc_reset_timer_start(info);
actual_pkt = (struct diag_pkt_frame_t *)buf;
for (i = 0; i < len; i++) {
if (actual_pkt->start == CONTROL_CHAR &&
actual_pkt->version == 1 &&
actual_pkt->length < len &&
(*(uint8_t *)(buf + sizeof(struct diag_pkt_frame_t) +
actual_pkt->length) == CONTROL_CHAR)) {
start_ptr = &buf[i];
break;
}
bad_byte_counter++;
if (bad_byte_counter > (DIAG_MAX_REQ_SIZE +
sizeof(struct diag_pkt_frame_t) + 1)) {
bad_byte_counter = 0;
pr_err("diag: In %s, re-enabling HDLC encoding\n",
__func__);
mutex_lock(&driver->hdlc_disable_mutex);
if (info)
info->hdlc_disabled = 0;
else
driver->hdlc_disabled = 0;
mutex_unlock(&driver->hdlc_disable_mutex);
diag_update_md_clients(HDLC_SUPPORT_TYPE);
return;
}
}
if (start_ptr) {
/* Discard any partial packet reads */
driver->incoming_pkt.processing = 0;
diag_process_non_hdlc_pkt(start_ptr, len - i, info);
}
}
void diag_process_non_hdlc_pkt(unsigned char *buf, int len,
struct diag_md_session_t *info)
{
int err = 0;
uint16_t pkt_len = 0;
uint32_t read_bytes = 0;
const uint32_t header_len = sizeof(struct diag_pkt_frame_t);
struct diag_pkt_frame_t *actual_pkt = NULL;
unsigned char *data_ptr = NULL;
struct diag_partial_pkt_t *partial_pkt = &driver->incoming_pkt;
if (!buf || len <= 0)
return;
if (!partial_pkt->processing)
goto start;
if (partial_pkt->remaining > len) {
if ((partial_pkt->read_len + len) > partial_pkt->capacity) {
pr_err("diag: Invalid length %d, %d received in %s\n",
partial_pkt->read_len, len, __func__);
goto end;
}
memcpy(partial_pkt->data + partial_pkt->read_len, buf, len);
read_bytes += len;
buf += read_bytes;
partial_pkt->read_len += len;
partial_pkt->remaining -= len;
} else {
if ((partial_pkt->read_len + partial_pkt->remaining) >
partial_pkt->capacity) {
pr_err("diag: Invalid length during partial read %d, %d received in %s\n",
partial_pkt->read_len,
partial_pkt->remaining, __func__);
goto end;
}
memcpy(partial_pkt->data + partial_pkt->read_len, buf,
partial_pkt->remaining);
read_bytes += partial_pkt->remaining;
buf += read_bytes;
partial_pkt->read_len += partial_pkt->remaining;
partial_pkt->remaining = 0;
}
if (partial_pkt->remaining == 0) {
actual_pkt = (struct diag_pkt_frame_t *)(partial_pkt->data);
data_ptr = partial_pkt->data + header_len;
if (*(uint8_t *)(data_ptr + actual_pkt->length) != CONTROL_CHAR)
diag_hdlc_start_recovery(buf, len, info);
err = diag_process_apps_pkt(data_ptr,
actual_pkt->length, info);
if (err) {
pr_err("diag: In %s, unable to process incoming data packet, err: %d\n",
__func__, err);
goto end;
}
partial_pkt->read_len = 0;
partial_pkt->total_len = 0;
partial_pkt->processing = 0;
goto start;
}
goto end;
start:
while (read_bytes < len) {
actual_pkt = (struct diag_pkt_frame_t *)buf;
pkt_len = actual_pkt->length;
if (actual_pkt->start != CONTROL_CHAR) {
diag_hdlc_start_recovery(buf, len, info);
diag_send_error_rsp(buf, len);
goto end;
}
if (pkt_len + header_len > partial_pkt->capacity) {
pr_err("diag: In %s, incoming data is too large for the request buffer %d\n",
__func__, pkt_len);
diag_hdlc_start_recovery(buf, len, info);
break;
}
if ((pkt_len + header_len) > (len - read_bytes)) {
partial_pkt->read_len = len - read_bytes;
partial_pkt->total_len = pkt_len + header_len;
partial_pkt->remaining = partial_pkt->total_len -
partial_pkt->read_len;
partial_pkt->processing = 1;
memcpy(partial_pkt->data, buf, partial_pkt->read_len);
break;
}
data_ptr = buf + header_len;
if (*(uint8_t *)(data_ptr + actual_pkt->length) != CONTROL_CHAR)
diag_hdlc_start_recovery(buf, len, info);
else
hdlc_reset = 0;
err = diag_process_apps_pkt(data_ptr,
actual_pkt->length, info);
if (err)
break;
read_bytes += header_len + pkt_len + 1;
buf += header_len + pkt_len + 1; /* advance to next pkt */
}
end:
return;
}
static int diagfwd_mux_read_done(unsigned char *buf, int len, int ctxt)
{
if (!buf || len <= 0)
return -EINVAL;
if (!driver->hdlc_disabled)
diag_process_hdlc_pkt(buf, len, NULL);
else
diag_process_non_hdlc_pkt(buf, len, NULL);
diag_mux_queue_read(ctxt);
return 0;
}
static int diagfwd_mux_write_done(unsigned char *buf, int len, int buf_ctxt,
int ctxt)
{
unsigned long flags;
int peripheral = -1;
int type = -1;
int num = -1;
if (!buf || len < 0)
return -EINVAL;
peripheral = GET_BUF_PERIPHERAL(buf_ctxt);
type = GET_BUF_TYPE(buf_ctxt);
num = GET_BUF_NUM(buf_ctxt);
switch (type) {
case TYPE_DATA:
if (peripheral >= 0 && peripheral < NUM_PERIPHERALS) {
diagfwd_write_done(peripheral, type, num);
diag_ws_on_copy(DIAG_WS_MUX);
} else if (peripheral == APPS_DATA) {
diagmem_free(driver, (unsigned char *)buf,
POOL_TYPE_HDLC);
buf = NULL;
} else {
pr_err_ratelimited("diag: Invalid peripheral %d in %s, type: %d\n",
peripheral, __func__, type);
}
break;
case TYPE_CMD:
if (peripheral >= 0 && peripheral < NUM_PERIPHERALS) {
diagfwd_write_done(peripheral, type, num);
} else if (peripheral == APPS_DATA) {
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:
pr_err_ratelimited("diag: Incorrect data type %d, buf_ctxt: %d in %s\n",
type, buf_ctxt, __func__);
break;
}
return 0;
}
static struct diag_mux_ops diagfwd_mux_ops = {
.open = diagfwd_mux_open,
.close = diagfwd_mux_close,
.read_done = diagfwd_mux_read_done,
.write_done = diagfwd_mux_write_done
};
int diagfwd_init(void)
{
int ret;
int i;
wrap_enabled = 0;
wrap_count = 0;
driver->use_device_tree = has_device_tree();
for (i = 0; i < DIAG_NUM_PROC; i++)
driver->real_time_mode[i] = 1;
driver->supports_separate_cmdrsp = 1;
driver->supports_apps_hdlc_encoding = 1;
mutex_init(&driver->diag_hdlc_mutex);
mutex_init(&driver->diag_cntl_mutex);
mutex_init(&driver->mode_lock);
driver->encoded_rsp_buf = kzalloc(DIAG_MAX_HDLC_BUF_SIZE +
APF_DIAG_PADDING, GFP_KERNEL);
if (!driver->encoded_rsp_buf)
goto err;
kmemleak_not_leak(driver->encoded_rsp_buf);
hdlc_decode = kzalloc(sizeof(struct diag_hdlc_decode_type),
GFP_KERNEL);
if (!hdlc_decode)
goto err;
setup_timer(&driver->hdlc_reset_timer, hdlc_reset_timer_func, 0);
kmemleak_not_leak(hdlc_decode);
driver->encoded_rsp_len = 0;
driver->rsp_buf_busy = 0;
spin_lock_init(&driver->rsp_buf_busy_lock);
driver->user_space_data_busy = 0;
driver->hdlc_buf_len = 0;
INIT_LIST_HEAD(&driver->cmd_reg_list);
driver->cmd_reg_count = 0;
mutex_init(&driver->cmd_reg_mutex);
for (i = 0; i < NUM_PERIPHERALS; i++) {
driver->feature[i].separate_cmd_rsp = 0;
driver->feature[i].stm_support = DISABLE_STM;
driver->feature[i].rcvd_feature_mask = 0;
driver->feature[i].peripheral_buffering = 0;
driver->feature[i].encode_hdlc = 0;
driver->feature[i].mask_centralization = 0;
driver->feature[i].log_on_demand = 0;
driver->feature[i].sent_feature_mask = 0;
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;
driver->buffering_mode[i].low_wm_val = DEFAULT_LOW_WM_VAL;
}
for (i = 0; i < NUM_STM_PROCESSORS; i++) {
driver->stm_state_requested[i] = DISABLE_STM;
driver->stm_state[i] = DISABLE_STM;
}
if (driver->hdlc_buf == NULL) {
driver->hdlc_buf = kzalloc(DIAG_MAX_HDLC_BUF_SIZE, GFP_KERNEL);
if (!driver->hdlc_buf)
goto err;
kmemleak_not_leak(driver->hdlc_buf);
}
if (driver->user_space_data_buf == NULL)
driver->user_space_data_buf = kzalloc(USER_SPACE_DATA,
GFP_KERNEL);
if (driver->user_space_data_buf == NULL)
goto err;
kmemleak_not_leak(driver->user_space_data_buf);
if (driver->client_map == NULL &&
(driver->client_map = kzalloc
((driver->num_clients) * sizeof(struct diag_client_map),
GFP_KERNEL)) == NULL)
goto err;
kmemleak_not_leak(driver->client_map);
if (driver->data_ready == NULL &&
(driver->data_ready = kzalloc(driver->num_clients * sizeof(int)
, GFP_KERNEL)) == NULL)
goto err;
kmemleak_not_leak(driver->data_ready);
if (driver->apps_req_buf == NULL) {
driver->apps_req_buf = kzalloc(DIAG_MAX_REQ_SIZE, GFP_KERNEL);
if (!driver->apps_req_buf)
goto err;
kmemleak_not_leak(driver->apps_req_buf);
}
if (driver->dci_pkt_buf == NULL) {
driver->dci_pkt_buf = kzalloc(DCI_BUF_SIZE, GFP_KERNEL);
if (!driver->dci_pkt_buf)
goto err;
kmemleak_not_leak(driver->dci_pkt_buf);
}
if (driver->apps_rsp_buf == NULL) {
driver->apps_rsp_buf = kzalloc(DIAG_MAX_RSP_SIZE, GFP_KERNEL);
if (driver->apps_rsp_buf == NULL)
goto err;
kmemleak_not_leak(driver->apps_rsp_buf);
}
driver->diag_wq = create_singlethread_workqueue("diag_wq");
if (!driver->diag_wq)
goto err;
ret = diag_mux_register(DIAG_LOCAL_PROC, DIAG_LOCAL_PROC,
&diagfwd_mux_ops);
if (ret) {
pr_err("diag: Unable to register with USB, err: %d\n", ret);
goto err;
}
return 0;
err:
pr_err("diag: In %s, couldn't initialize diag\n", __func__);
diag_usb_exit(DIAG_USB_LOCAL);
kfree(driver->encoded_rsp_buf);
kfree(driver->hdlc_buf);
kfree(driver->client_map);
kfree(driver->data_ready);
kfree(driver->apps_req_buf);
kfree(driver->dci_pkt_buf);
kfree(driver->apps_rsp_buf);
kfree(hdlc_decode);
kfree(driver->user_space_data_buf);
if (driver->diag_wq)
destroy_workqueue(driver->diag_wq);
return -ENOMEM;
}
void diagfwd_exit(void)
{
kfree(driver->encoded_rsp_buf);
kfree(driver->hdlc_buf);
kfree(hdlc_decode);
kfree(driver->client_map);
kfree(driver->data_ready);
kfree(driver->apps_req_buf);
kfree(driver->dci_pkt_buf);
kfree(driver->apps_rsp_buf);
kfree(driver->user_space_data_buf);
destroy_workqueue(driver->diag_wq);
}
Reference in New Issue View Git Blame Copy Permalink