M7350/kernel/drivers/char/diag/diagfwd_cntl.c
2024-09-09 08:52:07 +00:00

381 lines
10 KiB
C

/* Copyright (c) 2011-2013, 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/diagchar.h>
#include <linux/platform_device.h>
#include <linux/kmemleak.h>
#include <linux/delay.h>
#include "diagchar.h"
#include "diagfwd.h"
#include "diagfwd_cntl.h"
/* tracks which peripheral is undergoing SSR */
static uint16_t reg_dirty;
#define HDR_SIZ 8
void diag_clean_reg_fn(struct work_struct *work)
{
struct diag_smd_info *smd_info = container_of(work,
struct diag_smd_info,
diag_notify_update_smd_work);
if (!smd_info)
return;
pr_debug("diag: clean registration for peripheral: %d\n",
smd_info->peripheral);
reg_dirty |= smd_info->peripheral_mask;
diag_clear_reg(smd_info->peripheral);
reg_dirty ^= smd_info->peripheral_mask;
smd_info->notify_context = 0;
}
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)
{
int data_len = 0, type = -1, count_bytes = 0, j, flag = 0;
struct bindpkt_params_per_process *pkt_params =
kzalloc(sizeof(struct bindpkt_params_per_process), GFP_KERNEL);
struct diag_ctrl_msg *msg;
struct cmd_code_range *range;
struct bindpkt_params *temp;
if (pkt_params == NULL) {
pr_alert("diag: In %s, Memory allocation failure\n",
__func__);
return 0;
}
if (!smd_info) {
pr_err("diag: In %s, No smd info. Not able to read.\n",
__func__);
kfree(pkt_params);
return 0;
}
while (count_bytes + HDR_SIZ <= total_recd) {
type = *(uint32_t *)(buf);
data_len = *(uint32_t *)(buf + 4);
if (type < DIAG_CTRL_MSG_REG ||
type > DIAG_CTRL_MSG_LAST) {
pr_alert("diag: In %s, Invalid Msg type %d proc %d",
__func__, type, smd_info->peripheral);
break;
}
if (data_len < 0 || data_len > total_recd) {
pr_alert("diag: In %s, Invalid data len %d, total_recd: %d, proc %d",
__func__, data_len, total_recd,
smd_info->peripheral);
break;
}
count_bytes = count_bytes+HDR_SIZ+data_len;
if (type == DIAG_CTRL_MSG_REG && total_recd >= count_bytes) {
msg = buf+HDR_SIZ;
range = buf+HDR_SIZ+
sizeof(struct diag_ctrl_msg);
pkt_params->count = msg->count_entries;
pkt_params->params = kzalloc(pkt_params->count *
sizeof(struct bindpkt_params), GFP_KERNEL);
if (ZERO_OR_NULL_PTR(pkt_params->params)) {
pr_alert("diag: In %s, Memory alloc fail\n",
__func__);
kfree(pkt_params);
return flag;
}
temp = pkt_params->params;
for (j = 0; j < pkt_params->count; j++) {
temp->cmd_code = msg->cmd_code;
temp->subsys_id = msg->subsysid;
temp->client_id = smd_info->peripheral;
temp->proc_id = NON_APPS_PROC;
temp->cmd_code_lo = range->cmd_code_lo;
temp->cmd_code_hi = range->cmd_code_hi;
range++;
temp++;
}
flag = 1;
/* peripheral undergoing SSR should not
* record new registration
*/
if (!(reg_dirty & smd_info->peripheral_mask))
diagchar_ioctl(NULL, DIAG_IOCTL_COMMAND_REG,
(unsigned long)pkt_params);
else
pr_err("diag: drop reg proc %d\n",
smd_info->peripheral);
kfree(pkt_params->params);
} else if (type == DIAG_CTRL_MSG_FEATURE &&
total_recd >= count_bytes) {
uint8_t feature_mask = 0;
int feature_mask_len = *(int *)(buf+8);
if (feature_mask_len > 0) {
feature_mask = *(uint8_t *)(buf+12);
if (smd_info->peripheral == MODEM_DATA)
driver->log_on_demand_support =
feature_mask &
F_DIAG_LOG_ON_DEMAND_RSP_ON_MASTER;
/*
* If apps supports separate cmd/rsp channels
* and the peripheral supports separate cmd/rsp
* channels
*/
if (driver->supports_separate_cmdrsp &&
(feature_mask & F_DIAG_REQ_RSP_CHANNEL))
driver->separate_cmdrsp
[smd_info->peripheral] =
ENABLE_SEPARATE_CMDRSP;
else
driver->separate_cmdrsp
[smd_info->peripheral] =
DISABLE_SEPARATE_CMDRSP;
/*
* Check if apps supports hdlc encoding and the
* peripheral supports apps hdlc encoding
*/
process_hdlc_encoding_feature(smd_info,
feature_mask);
}
flag = 1;
} else if (type != DIAG_CTRL_MSG_REG) {
flag = 1;
}
buf = buf + HDR_SIZ + data_len;
}
kfree(pkt_params);
return flag;
}
void diag_send_diag_mode_update(int real_time)
{
int i;
for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++)
diag_send_diag_mode_update_by_smd(&driver->smd_cntl[i],
real_time);
}
void diag_send_diag_mode_update_by_smd(struct diag_smd_info *smd_info,
int real_time)
{
struct diag_ctrl_msg_diagmode diagmode;
char buf[sizeof(struct diag_ctrl_msg_diagmode)];
int msg_size = sizeof(struct diag_ctrl_msg_diagmode);
int wr_size = -ENOMEM, retry_count = 0, timer;
/* For now only allow the modem to receive the message */
if (!smd_info || smd_info->type != SMD_CNTL_TYPE ||
(smd_info->peripheral != MODEM_DATA))
return;
mutex_lock(&driver->diag_cntl_mutex);
diagmode.ctrl_pkt_id = DIAG_CTRL_MSG_DIAGMODE;
diagmode.ctrl_pkt_data_len = 36;
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(buf, &diagmode, msg_size);
if (smd_info->ch) {
while (retry_count < 3) {
wr_size = smd_write(smd_info->ch, buf, 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 2000 was determined empirically as
* best value to use.
*/
retry_count++;
for (timer = 0; timer < 5; timer++)
udelay(2000);
} else {
struct diag_smd_info *data =
&driver->smd_data[smd_info->peripheral];
driver->real_time_mode = real_time;
process_lock_enabling(&data->nrt_lock,
real_time);
break;
}
}
if (wr_size != msg_size)
pr_err("diag: proc %d fail feature update %d, tried %d",
smd_info->peripheral,
wr_size, msg_size);
} else {
pr_err("diag: ch invalid, feature update on proc %d\n",
smd_info->peripheral);
}
mutex_unlock(&driver->diag_cntl_mutex);
}
static int diag_smd_cntl_probe(struct platform_device *pdev)
{
int r = 0;
int index = -1;
const char *channel_name = NULL;
/* open control ports only on 8960 & newer targets */
if (chk_apps_only()) {
if (pdev->id == SMD_APPS_MODEM) {
index = MODEM_DATA;
channel_name = "DIAG_CNTL";
}
#if defined(CONFIG_MSM_N_WAY_SMD)
else if (pdev->id == SMD_APPS_QDSP) {
index = LPASS_DATA;
channel_name = "DIAG_CNTL";
}
#endif
else if (pdev->id == SMD_APPS_WCNSS) {
index = WCNSS_DATA;
channel_name = "APPS_RIVA_CTRL";
}
if (index != -1) {
r = smd_named_open_on_edge(channel_name,
pdev->id,
&driver->smd_cntl[index].ch,
&driver->smd_cntl[index],
diag_smd_notify);
driver->smd_cntl[index].ch_save =
driver->smd_cntl[index].ch;
}
pr_debug("diag: In %s, open SMD CNTL port, Id = %d, r = %d\n",
__func__, pdev->id, r);
}
return 0;
}
static int diagfwd_cntl_runtime_suspend(struct device *dev)
{
dev_dbg(dev, "pm_runtime: suspending...\n");
return 0;
}
static int diagfwd_cntl_runtime_resume(struct device *dev)
{
dev_dbg(dev, "pm_runtime: resuming...\n");
return 0;
}
static const struct dev_pm_ops diagfwd_cntl_dev_pm_ops = {
.runtime_suspend = diagfwd_cntl_runtime_suspend,
.runtime_resume = diagfwd_cntl_runtime_resume,
};
static struct platform_driver msm_smd_ch1_cntl_driver = {
.probe = diag_smd_cntl_probe,
.driver = {
.name = "DIAG_CNTL",
.owner = THIS_MODULE,
.pm = &diagfwd_cntl_dev_pm_ops,
},
};
static struct platform_driver diag_smd_lite_cntl_driver = {
.probe = diag_smd_cntl_probe,
.driver = {
.name = "APPS_RIVA_CTRL",
.owner = THIS_MODULE,
.pm = &diagfwd_cntl_dev_pm_ops,
},
};
void diagfwd_cntl_init(void)
{
int success;
int i;
reg_dirty = 0;
driver->polling_reg_flag = 0;
driver->log_on_demand_support = 1;
driver->diag_cntl_wq = create_singlethread_workqueue("diag_cntl_wq");
for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
success = diag_smd_constructor(&driver->smd_cntl[i], i,
SMD_CNTL_TYPE);
if (!success)
goto err;
}
platform_driver_register(&msm_smd_ch1_cntl_driver);
platform_driver_register(&diag_smd_lite_cntl_driver);
return;
err:
pr_err("diag: Could not initialize diag buffers");
for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++)
diag_smd_destructor(&driver->smd_cntl[i]);
if (driver->diag_cntl_wq)
destroy_workqueue(driver->diag_cntl_wq);
}
void diagfwd_cntl_exit(void)
{
int i;
for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++)
diag_smd_destructor(&driver->smd_cntl[i]);
destroy_workqueue(driver->diag_cntl_wq);
platform_driver_unregister(&msm_smd_ch1_cntl_driver);
platform_driver_unregister(&diag_smd_lite_cntl_driver);
}