blob: 9730d4f2ced430e1d24fc06a3c203e8b84c068e2 [file] [log] [blame]
/* Copyright (c) 2011, Code Aurora Forum. 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 "diagchar.h"
#include "diagfwd.h"
#include "diagfwd_cntl.h"
#define HDR_SIZ 8
static void diag_smd_cntl_send_req(int proc_num)
{
int data_len = 0, type = -1, count_bytes = 0, j, r, 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;
void *buf = NULL;
smd_channel_t *smd_ch = NULL;
if (proc_num == MODEM_PROC) {
buf = driver->buf_in_cntl;
smd_ch = driver->ch_cntl;
} else if (proc_num == QDSP_PROC) {
buf = driver->buf_in_qdsp_cntl;
smd_ch = driver->chqdsp_cntl;
} else if (proc_num == WCNSS_PROC) {
buf = driver->buf_in_wcnss_cntl;
smd_ch = driver->ch_wcnss_cntl;
}
if (!smd_ch || !buf) {
kfree(pkt_params);
return;
}
r = smd_read_avail(smd_ch);
if (r > IN_BUF_SIZE) {
if (r < MAX_IN_BUF_SIZE) {
pr_err("diag: SMD CNTL sending pkt upto %d bytes", r);
buf = krealloc(buf, r, GFP_KERNEL);
} else {
pr_err("diag: CNTL pkt > %d bytes", MAX_IN_BUF_SIZE);
kfree(pkt_params);
return;
}
}
if (buf && r > 0) {
smd_read(smd_ch, buf, r);
while (count_bytes + HDR_SIZ <= r) {
type = *(uint32_t *)(buf);
data_len = *(uint32_t *)(buf + 4);
count_bytes = count_bytes+HDR_SIZ+data_len;
if (type == DIAG_CTRL_MSG_REG && r >= count_bytes) {
msg = buf+HDR_SIZ;
range = buf+HDR_SIZ+
sizeof(struct diag_ctrl_msg);
pkt_params->count = msg->count_entries;
temp = kzalloc(pkt_params->count * sizeof(struct
bindpkt_params), GFP_KERNEL);
for (j = 0; j < pkt_params->count; j++) {
temp->cmd_code = msg->cmd_code;
temp->subsys_id = msg->subsysid;
temp->client_id = proc_num;
temp->proc_id = proc_num;
temp->cmd_code_lo = range->cmd_code_lo;
temp->cmd_code_hi = range->cmd_code_hi;
range++;
temp++;
}
temp -= pkt_params->count;
pkt_params->params = temp;
flag = 1;
diagchar_ioctl(NULL, DIAG_IOCTL_COMMAND_REG,
(unsigned long)pkt_params);
kfree(temp);
buf = buf + HDR_SIZ + data_len;
}
}
}
kfree(pkt_params);
if (flag) {
/* Poll SMD CNTL channels to check for data */
queue_work(driver->diag_wq, &(driver->diag_read_smd_cntl_work));
queue_work(driver->diag_wq,
&(driver->diag_read_smd_qdsp_cntl_work));
queue_work(driver->diag_wq,
&(driver->diag_read_smd_wcnss_cntl_work));
}
}
void diag_read_smd_cntl_work_fn(struct work_struct *work)
{
diag_smd_cntl_send_req(MODEM_PROC);
}
void diag_read_smd_qdsp_cntl_work_fn(struct work_struct *work)
{
diag_smd_cntl_send_req(QDSP_PROC);
}
void diag_read_smd_wcnss_cntl_work_fn(struct work_struct *work)
{
diag_smd_cntl_send_req(WCNSS_PROC);
}
static void diag_smd_cntl_notify(void *ctxt, unsigned event)
{
queue_work(driver->diag_wq, &(driver->diag_read_smd_cntl_work));
}
static void diag_smd_qdsp_cntl_notify(void *ctxt, unsigned event)
{
queue_work(driver->diag_wq, &(driver->diag_read_smd_qdsp_cntl_work));
}
static void diag_smd_wcnss_cntl_notify(void *ctxt, unsigned event)
{
queue_work(driver->diag_wq, &(driver->diag_read_smd_wcnss_cntl_work));
}
static int diag_smd_cntl_probe(struct platform_device *pdev)
{
int r = 0;
/* open control ports only on 8960 */
if (chk_apps_only()) {
if (pdev->id == SMD_APPS_MODEM)
r = smd_open("DIAG_CNTL", &driver->ch_cntl, driver,
diag_smd_cntl_notify);
if (pdev->id == SMD_APPS_QDSP)
r = smd_named_open_on_edge("DIAG_CNTL", SMD_APPS_QDSP
, &driver->chqdsp_cntl, driver,
diag_smd_qdsp_cntl_notify);
if (pdev->id == SMD_APPS_WCNSS)
r = smd_named_open_on_edge("APPS_RIVA_CTRL",
SMD_APPS_WCNSS, &driver->ch_wcnss_cntl,
driver, diag_smd_wcnss_cntl_notify);
pr_debug("diag: open CNTL port, ID = %d,r = %d\n", 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)
{
if (driver->buf_in_cntl == NULL) {
driver->buf_in_cntl = kzalloc(IN_BUF_SIZE, GFP_KERNEL);
if (driver->buf_in_cntl == NULL)
goto err;
}
if (driver->buf_in_qdsp_cntl == NULL) {
driver->buf_in_qdsp_cntl = kzalloc(IN_BUF_SIZE, GFP_KERNEL);
if (driver->buf_in_qdsp_cntl == NULL)
goto err;
}
if (driver->buf_in_wcnss_cntl == NULL) {
driver->buf_in_wcnss_cntl = kzalloc(IN_BUF_SIZE, GFP_KERNEL);
if (driver->buf_in_wcnss_cntl == NULL)
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");
kfree(driver->buf_in_cntl);
kfree(driver->buf_in_qdsp_cntl);
kfree(driver->buf_in_wcnss_cntl);
}
void diagfwd_cntl_exit(void)
{
smd_close(driver->ch_cntl);
smd_close(driver->chqdsp_cntl);
smd_close(driver->ch_wcnss_cntl);
driver->ch_cntl = 0;
driver->chqdsp_cntl = 0;
driver->ch_wcnss_cntl = 0;
platform_driver_unregister(&msm_smd_ch1_cntl_driver);
platform_driver_unregister(&diag_smd_lite_cntl_driver);
kfree(driver->buf_in_cntl);
kfree(driver->buf_in_qdsp_cntl);
kfree(driver->buf_in_wcnss_cntl);
}