diff --git a/drivers/coresight/coresight-audio-etm.c b/drivers/coresight/coresight-audio-etm.c index cdf44bfa66f..be270058b66 100644 --- a/drivers/coresight/coresight-audio-etm.c +++ b/drivers/coresight/coresight-audio-etm.c @@ -1,4 +1,4 @@ -/* Copyright (c) 2013, The Linux Foundation. All rights reserved. +/* Copyright (c) 2013-2014, 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 @@ -19,30 +19,110 @@ #include #include #include +#include #include #include +#include "coresight-qmi.h" struct audio_etm_drvdata { struct device *dev; struct coresight_device *csdev; + struct mutex mutex; + struct workqueue_struct *wq; + struct qmi_handle *handle; + struct work_struct work_svc_arrive; + struct work_struct work_svc_exit; + struct work_struct work_rcv_msg; + struct notifier_block nb; }; static int audio_etm_enable(struct coresight_device *csdev) { struct audio_etm_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent); + struct coresight_set_etm_req_msg_v01 req; + struct coresight_set_etm_resp_msg_v01 resp = { { 0, 0 } }; + struct msg_desc req_desc, resp_desc; + int ret; + + mutex_lock(&drvdata->mutex); + + req.state = CORESIGHT_ETM_STATE_ENABLED_V01; + + req_desc.msg_id = CORESIGHT_QMI_SET_ETM_REQ_V01; + req_desc.max_msg_len = CORESIGHT_QMI_SET_ETM_REQ_MAX_LEN; + req_desc.ei_array = coresight_set_etm_req_msg_v01_ei; + + resp_desc.msg_id = CORESIGHT_QMI_SET_ETM_RESP_V01; + resp_desc.max_msg_len = CORESIGHT_QMI_SET_ETM_RESP_MAX_LEN; + resp_desc.ei_array = coresight_set_etm_resp_msg_v01_ei; + + ret = qmi_send_req_wait(drvdata->handle, &req_desc, &req, sizeof(req), + &resp_desc, &resp, sizeof(resp), TIMEOUT_MS); + + if (ret < 0) { + dev_err(drvdata->dev, "%s: QMI send req failed %d\n", __func__, + ret); + goto err; + } + + if (resp.resp.result != QMI_RESULT_SUCCESS_V01) { + dev_err(drvdata->dev, "%s: QMI request failed %d %d\n", + __func__, resp.resp.result, resp.resp.error); + ret = -EREMOTEIO; + goto err; + } + + mutex_unlock(&drvdata->mutex); dev_info(drvdata->dev, "Audio ETM tracing enabled\n"); return 0; +err: + mutex_unlock(&drvdata->mutex); + return ret; } - static void audio_etm_disable(struct coresight_device *csdev) { struct audio_etm_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent); + struct coresight_set_etm_req_msg_v01 req; + struct coresight_set_etm_resp_msg_v01 resp = { { 0, 0 } }; + struct msg_desc req_desc, resp_desc; + int ret; + + mutex_lock(&drvdata->mutex); + + req.state = CORESIGHT_ETM_STATE_DISABLED_V01; + + req_desc.msg_id = CORESIGHT_QMI_SET_ETM_REQ_V01; + req_desc.max_msg_len = CORESIGHT_QMI_SET_ETM_REQ_MAX_LEN; + req_desc.ei_array = coresight_set_etm_req_msg_v01_ei; + + resp_desc.msg_id = CORESIGHT_QMI_SET_ETM_RESP_V01; + resp_desc.max_msg_len = CORESIGHT_QMI_SET_ETM_RESP_MAX_LEN; + resp_desc.ei_array = coresight_set_etm_resp_msg_v01_ei; + + ret = qmi_send_req_wait(drvdata->handle, &req_desc, &req, sizeof(req), + &resp_desc, &resp, sizeof(resp), TIMEOUT_MS); + if (ret < 0) { + dev_err(drvdata->dev, "%s: QMI send req failed %d\n", __func__, + ret); + goto err; + } + + if (resp.resp.result != QMI_RESULT_SUCCESS_V01) { + dev_err(drvdata->dev, "%s: QMI request failed %d %d\n", + __func__, resp.resp.result, resp.resp.error); + goto err; + } + + mutex_unlock(&drvdata->mutex); dev_info(drvdata->dev, "Audio ETM tracing disabled\n"); + return; +err: + mutex_unlock(&drvdata->mutex); } static const struct coresight_ops_source audio_etm_source_ops = { @@ -54,12 +134,89 @@ static const struct coresight_ops audio_cs_ops = { .source_ops = &audio_etm_source_ops, }; +static void audio_etm_rcv_msg(struct work_struct *work) +{ + struct audio_etm_drvdata *drvdata = container_of(work, + struct audio_etm_drvdata, + work_rcv_msg); + + if (qmi_recv_msg(drvdata->handle) < 0) + pr_err("%s: Error receiving QMI message\n", __func__); +} + +static void audio_etm_notify(struct qmi_handle *handle, + enum qmi_event_type event, void *notify_priv) +{ + struct audio_etm_drvdata *drvdata = + (struct audio_etm_drvdata *)notify_priv; + switch (event) { + case QMI_RECV_MSG: + queue_work(drvdata->wq, &drvdata->work_rcv_msg); + break; + default: + break; + } +} + +static void audio_etm_svc_arrive(struct work_struct *work) +{ + struct audio_etm_drvdata *drvdata = container_of(work, + struct audio_etm_drvdata, + work_svc_arrive); + + drvdata->handle = qmi_handle_create(audio_etm_notify, drvdata); + if (!drvdata->handle) { + pr_err("%s: QMI client handle alloc failed\n", __func__); + return; + } + + if (qmi_connect_to_service(drvdata->handle, CORESIGHT_QMI_SVC_ID, + CORESIGHT_QMI_VERSION, + CORESIGHT_SVC_INST_ID_AUDIO_V01) < 0) { + pr_err("%s: Could not connect handle to service\n", __func__); + qmi_handle_destroy(drvdata->handle); + drvdata->handle = NULL; + } +} + +static void audio_etm_svc_exit(struct work_struct *work) +{ + struct audio_etm_drvdata *drvdata = container_of(work, + struct audio_etm_drvdata, + work_svc_exit); + + qmi_handle_destroy(drvdata->handle); + drvdata->handle = NULL; +} + +static int audio_etm_svc_event_notify(struct notifier_block *this, + unsigned long event, + void *data) +{ + struct audio_etm_drvdata *drvdata = container_of(this, + struct audio_etm_drvdata, + nb); + + switch (event) { + case QMI_SERVER_ARRIVE: + queue_work(drvdata->wq, &drvdata->work_svc_arrive); + break; + case QMI_SERVER_EXIT: + queue_work(drvdata->wq, &drvdata->work_svc_exit); + break; + default: + break; + } + return 0; +} + static int audio_etm_probe(struct platform_device *pdev) { struct device *dev = &pdev->dev; struct coresight_platform_data *pdata; struct audio_etm_drvdata *drvdata; struct coresight_desc *desc; + int ret; if (pdev->dev.of_node) { pdata = of_get_coresight_platform_data(dev, pdev->dev.of_node); @@ -79,6 +236,23 @@ static int audio_etm_probe(struct platform_device *pdev) if (!desc) return -ENOMEM; + mutex_init(&drvdata->mutex); + + drvdata->nb.notifier_call = audio_etm_svc_event_notify; + + drvdata->wq = create_singlethread_workqueue("audio-etm"); + if (!drvdata->wq) + return -EFAULT; + INIT_WORK(&drvdata->work_svc_arrive, audio_etm_svc_arrive); + INIT_WORK(&drvdata->work_svc_exit, audio_etm_svc_exit); + INIT_WORK(&drvdata->work_rcv_msg, audio_etm_rcv_msg); + ret = qmi_svc_event_notifier_register(CORESIGHT_QMI_SVC_ID, + CORESIGHT_QMI_VERSION, + CORESIGHT_SVC_INST_ID_AUDIO_V01, + &drvdata->nb); + if (ret < 0) + goto err0; + desc->type = CORESIGHT_DEV_TYPE_SOURCE; desc->subtype.source_subtype = CORESIGHT_DEV_SUBTYPE_SOURCE_PROC; desc->ops = &audio_cs_ops; @@ -86,11 +260,20 @@ static int audio_etm_probe(struct platform_device *pdev) desc->dev = &pdev->dev; desc->owner = THIS_MODULE; drvdata->csdev = coresight_register(desc); - if (IS_ERR(drvdata->csdev)) - return PTR_ERR(drvdata->csdev); - + if (IS_ERR(drvdata->csdev)) { + ret = PTR_ERR(drvdata->csdev); + goto err1; + } dev_info(dev, "Audio ETM initialized\n"); return 0; +err1: + qmi_svc_event_notifier_unregister(CORESIGHT_QMI_SVC_ID, + CORESIGHT_QMI_VERSION, + CORESIGHT_SVC_INST_ID_AUDIO_V01, + &drvdata->nb); +err0: + destroy_workqueue(drvdata->wq); + return ret; } static int audio_etm_remove(struct platform_device *pdev) diff --git a/drivers/coresight/coresight-modem-etm.c b/drivers/coresight/coresight-modem-etm.c index 4c8075c2996..b2ad8ff4f1d 100644 --- a/drivers/coresight/coresight-modem-etm.c +++ b/drivers/coresight/coresight-modem-etm.c @@ -1,4 +1,4 @@ -/* Copyright (c) 2013, The Linux Foundation. All rights reserved. +/* Copyright (c) 2013-2014, 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 @@ -19,21 +19,67 @@ #include #include #include +#include #include #include +#include "coresight-qmi.h" struct modem_etm_drvdata { struct device *dev; struct coresight_device *csdev; + struct mutex mutex; + struct workqueue_struct *wq; + struct qmi_handle *handle; + struct work_struct work_svc_arrive; + struct work_struct work_svc_exit; + struct work_struct work_rcv_msg; + struct notifier_block nb; }; static int modem_etm_enable(struct coresight_device *csdev) { struct modem_etm_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent); + struct coresight_set_etm_req_msg_v01 req; + struct coresight_set_etm_resp_msg_v01 resp = { { 0, 0 } }; + struct msg_desc req_desc, resp_desc; + int ret; + + mutex_lock(&drvdata->mutex); + + req.state = CORESIGHT_ETM_STATE_ENABLED_V01; + + req_desc.msg_id = CORESIGHT_QMI_SET_ETM_REQ_V01; + req_desc.max_msg_len = CORESIGHT_QMI_SET_ETM_REQ_MAX_LEN; + req_desc.ei_array = coresight_set_etm_req_msg_v01_ei; + + resp_desc.msg_id = CORESIGHT_QMI_SET_ETM_RESP_V01; + resp_desc.max_msg_len = CORESIGHT_QMI_SET_ETM_RESP_MAX_LEN; + resp_desc.ei_array = coresight_set_etm_resp_msg_v01_ei; + + ret = qmi_send_req_wait(drvdata->handle, &req_desc, &req, sizeof(req), + &resp_desc, &resp, sizeof(resp), TIMEOUT_MS); + + if (ret < 0) { + dev_err(drvdata->dev, "%s: QMI send req failed %d\n", __func__, + ret); + goto err; + } + + if (resp.resp.result != QMI_RESULT_SUCCESS_V01) { + dev_err(drvdata->dev, "%s: QMI request failed %d %d\n", + __func__, resp.resp.result, resp.resp.error); + ret = -EREMOTEIO; + goto err; + } + + mutex_unlock(&drvdata->mutex); dev_info(drvdata->dev, "Modem ETM tracing enabled\n"); return 0; +err: + mutex_unlock(&drvdata->mutex); + return ret; } @@ -41,8 +87,43 @@ static void modem_etm_disable(struct coresight_device *csdev) { struct modem_etm_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent); + struct coresight_set_etm_req_msg_v01 req; + struct coresight_set_etm_resp_msg_v01 resp = { { 0, 0 } }; + struct msg_desc req_desc, resp_desc; + int ret; + + mutex_lock(&drvdata->mutex); + + req.state = CORESIGHT_ETM_STATE_DISABLED_V01; + + req_desc.msg_id = CORESIGHT_QMI_SET_ETM_REQ_V01; + req_desc.max_msg_len = CORESIGHT_QMI_SET_ETM_REQ_MAX_LEN; + req_desc.ei_array = coresight_set_etm_req_msg_v01_ei; + + resp_desc.msg_id = CORESIGHT_QMI_SET_ETM_RESP_V01; + resp_desc.max_msg_len = CORESIGHT_QMI_SET_ETM_RESP_MAX_LEN; + resp_desc.ei_array = coresight_set_etm_resp_msg_v01_ei; + + ret = qmi_send_req_wait(drvdata->handle, &req_desc, &req, sizeof(req), + &resp_desc, &resp, sizeof(resp), TIMEOUT_MS); + if (ret < 0) { + dev_err(drvdata->dev, "%s: QMI send req failed %d\n", __func__, + ret); + goto err; + } + + if (resp.resp.result != QMI_RESULT_SUCCESS_V01) { + dev_err(drvdata->dev, "%s: QMI request failed %d %d\n", + __func__, resp.resp.result, resp.resp.error); + goto err; + } + + mutex_unlock(&drvdata->mutex); dev_info(drvdata->dev, "Modem ETM tracing disabled\n"); + return; +err: + mutex_unlock(&drvdata->mutex); } static const struct coresight_ops_source modem_etm_source_ops = { @@ -54,12 +135,89 @@ static const struct coresight_ops modem_cs_ops = { .source_ops = &modem_etm_source_ops, }; +static void modem_etm_rcv_msg(struct work_struct *work) +{ + struct modem_etm_drvdata *drvdata = container_of(work, + struct modem_etm_drvdata, + work_rcv_msg); + + if (qmi_recv_msg(drvdata->handle) < 0) + pr_err("%s: Error receiving QMI message\n", __func__); +} + +static void modem_etm_notify(struct qmi_handle *handle, + enum qmi_event_type event, void *notify_priv) +{ + struct modem_etm_drvdata *drvdata = + (struct modem_etm_drvdata *)notify_priv; + switch (event) { + case QMI_RECV_MSG: + queue_work(drvdata->wq, &drvdata->work_rcv_msg); + break; + default: + break; + } +} + +static void modem_etm_svc_arrive(struct work_struct *work) +{ + struct modem_etm_drvdata *drvdata = container_of(work, + struct modem_etm_drvdata, + work_svc_arrive); + + drvdata->handle = qmi_handle_create(modem_etm_notify, drvdata); + if (!drvdata->handle) { + pr_err("%s: QMI client handle alloc failed\n", __func__); + return; + } + + if (qmi_connect_to_service(drvdata->handle, CORESIGHT_QMI_SVC_ID, + CORESIGHT_QMI_VERSION, + CORESIGHT_SVC_INST_ID_MODEM_V01) < 0) { + pr_err("%s: Could not connect handle to service\n", __func__); + qmi_handle_destroy(drvdata->handle); + drvdata->handle = NULL; + } +} + +static void modem_etm_svc_exit(struct work_struct *work) +{ + struct modem_etm_drvdata *drvdata = container_of(work, + struct modem_etm_drvdata, + work_svc_exit); + + qmi_handle_destroy(drvdata->handle); + drvdata->handle = NULL; +} + +static int modem_etm_svc_event_notify(struct notifier_block *this, + unsigned long event, + void *data) +{ + struct modem_etm_drvdata *drvdata = container_of(this, + struct modem_etm_drvdata, + nb); + + switch (event) { + case QMI_SERVER_ARRIVE: + queue_work(drvdata->wq, &drvdata->work_svc_arrive); + break; + case QMI_SERVER_EXIT: + queue_work(drvdata->wq, &drvdata->work_svc_exit); + break; + default: + break; + } + return 0; +} + static int modem_etm_probe(struct platform_device *pdev) { struct device *dev = &pdev->dev; struct coresight_platform_data *pdata; struct modem_etm_drvdata *drvdata; struct coresight_desc *desc; + int ret; if (pdev->dev.of_node) { pdata = of_get_coresight_platform_data(dev, pdev->dev.of_node); @@ -79,6 +237,23 @@ static int modem_etm_probe(struct platform_device *pdev) if (!desc) return -ENOMEM; + mutex_init(&drvdata->mutex); + + drvdata->nb.notifier_call = modem_etm_svc_event_notify; + + drvdata->wq = create_singlethread_workqueue("modem-etm"); + if (!drvdata->wq) + return -EFAULT; + INIT_WORK(&drvdata->work_svc_arrive, modem_etm_svc_arrive); + INIT_WORK(&drvdata->work_svc_exit, modem_etm_svc_exit); + INIT_WORK(&drvdata->work_rcv_msg, modem_etm_rcv_msg); + ret = qmi_svc_event_notifier_register(CORESIGHT_QMI_SVC_ID, + CORESIGHT_QMI_VERSION, + CORESIGHT_SVC_INST_ID_MODEM_V01, + &drvdata->nb); + if (ret < 0) + goto err0; + desc->type = CORESIGHT_DEV_TYPE_SOURCE; desc->subtype.source_subtype = CORESIGHT_DEV_SUBTYPE_SOURCE_PROC; desc->ops = &modem_cs_ops; @@ -86,11 +261,20 @@ static int modem_etm_probe(struct platform_device *pdev) desc->dev = &pdev->dev; desc->owner = THIS_MODULE; drvdata->csdev = coresight_register(desc); - if (IS_ERR(drvdata->csdev)) - return PTR_ERR(drvdata->csdev); - + if (IS_ERR(drvdata->csdev)) { + ret = PTR_ERR(drvdata->csdev); + goto err1; + } dev_info(dev, "Modem ETM initialized\n"); return 0; +err1: + qmi_svc_event_notifier_unregister(CORESIGHT_QMI_SVC_ID, + CORESIGHT_QMI_VERSION, + CORESIGHT_SVC_INST_ID_MODEM_V01, + &drvdata->nb); +err0: + destroy_workqueue(drvdata->wq); + return ret; } static int modem_etm_remove(struct platform_device *pdev) diff --git a/drivers/coresight/coresight-qmi.h b/drivers/coresight/coresight-qmi.h new file mode 100644 index 00000000000..0154efdcd15 --- /dev/null +++ b/drivers/coresight/coresight-qmi.h @@ -0,0 +1,132 @@ +/* Copyright (c) 2014, 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. + */ + +#ifndef _CORESIGHT_QMI_H +#define _CORESIGHT_QMI_H + +#include + +#define CORESIGHT_QMI_SVC_ID (0x33) +#define CORESIGHT_QMI_VERSION (1) + +#define CORESIGHT_QMI_GET_ETM_REQ_V01 (0x002B) +#define CORESIGHT_QMI_GET_ETM_RESP_V01 (0x002B) +#define CORESIGHT_QMI_SET_ETM_REQ_V01 (0x002C) +#define CORESIGHT_QMI_SET_ETM_RESP_V01 (0x002C) + +#define CORESIGHT_QMI_GET_ETM_REQ_MAX_LEN (0) +#define CORESIGHT_QMI_GET_ETM_RESP_MAX_LEN (14) +#define CORESIGHT_QMI_SET_ETM_REQ_MAX_LEN (7) +#define CORESIGHT_QMI_SET_ETM_RESP_MAX_LEN (7) + +#define TIMEOUT_MS (5000) + +enum coresight_svc_instance_id { + /* To force a 32 bit signed enum. Do not change or use */ + CORESIGHT_SVC_INST_ID_ENUM_TYPE_MIN_ENUM_VAL_V01 = INT_MIN, + CORESIGHT_SVC_INST_ID_UNKNOWN_V01 = 0, + CORESIGHT_SVC_INST_ID_APPS_V01 = 1, + CORESIGHT_SVC_INST_ID_MODEM_V01 = 2, + CORESIGHT_SVC_INST_ID_WCN_V01 = 3, + CORESIGHT_SVC_INST_ID_RPM_V01 = 4, + CORESIGHT_SVC_INST_ID_AUDIO_V01 = 5, + CORESIGHT_SVC_INST_ID_VIDEO_V01 = 6, + CORESIGHT_SVC_INST_ID_GNSS_V01 = 7, + CORESIGHT_SVC_INST_ID_SENSOR_V01 = 8, + CORESIGHT_SVC_INST_ID_VPU_V01 = 10, + CORESIGHT_SVC_INST_ID_ENUM_TYPE_MAX_ENUM_VAL_V01 = INT_MAX, +}; + +enum coresight_etm_state_enum_type_v01 { + /* To force a 32 bit signed enum. Do not change or use */ + CORESIGHT_ETM_STATE_ENUM_TYPE_MIN_ENUM_VAL_V01 = INT_MIN, + CORESIGHT_ETM_STATE_DISABLED_V01 = 0, + CORESIGHT_ETM_STATE_ENABLED_V01 = 1, + CORESIGHT_ETM_STATE_ENUM_TYPE_MAX_ENUM_VAL_01 = INT_MAX, +}; + +struct coresight_get_etm_req_msg_v01 { + /* + * This element is a placeholder to prevent declaration of + * empty struct. Do not change. + */ + char __placeholder; +}; + +struct coresight_get_etm_resp_msg_v01 { + /* Mandatory */ + /* QMI result Code */ + struct qmi_response_type_v01 resp; + + /* Optional */ + /* ETM output state, must be set to true if state is being passed */ + uint8_t state_valid; + /* Present when result code is QMI_RESULT_SUCCESS */ + enum coresight_etm_state_enum_type_v01 state; +}; + +struct coresight_set_etm_req_msg_v01 { + /* Mandatory */ + /* ETM output state */ + enum coresight_etm_state_enum_type_v01 state; +}; + +struct coresight_set_etm_resp_msg_v01 { + /* Mandatory */ + struct qmi_response_type_v01 resp; +}; + +static struct elem_info coresight_set_etm_req_msg_v01_ei[] = { + { + .data_type = QMI_UNSIGNED_4_BYTE, + .elem_len = 1, + .elem_size = sizeof(enum coresight_etm_state_enum_type_v01), + .is_array = NO_ARRAY, + .tlv_type = 0x01, + .offset = offsetof(struct coresight_set_etm_req_msg_v01, + state), + .ei_array = NULL, + }, + { + .data_type = QMI_EOTI, + .elem_len = 0, + .elem_size = 0, + .is_array = NO_ARRAY, + .tlv_type = 0, + .offset = 0, + .ei_array = NULL, + }, +}; + +static struct elem_info coresight_set_etm_resp_msg_v01_ei[] = { + { + .data_type = QMI_STRUCT, + .elem_len = 1, + .elem_size = sizeof(struct qmi_response_type_v01), + .is_array = NO_ARRAY, + .tlv_type = 0x02, + .offset = offsetof(struct coresight_set_etm_resp_msg_v01, + resp), + .ei_array = get_qmi_response_type_v01_ei(), + }, + { + .data_type = QMI_EOTI, + .elem_len = 0, + .elem_size = 0, + .is_array = NO_ARRAY, + .tlv_type = 0, + .offset = 0, + .ei_array = NULL, + }, +}; + +#endif diff --git a/drivers/coresight/coresight-rpm-etm.c b/drivers/coresight/coresight-rpm-etm.c index e752e4f57e2..93efd65a6d3 100644 --- a/drivers/coresight/coresight-rpm-etm.c +++ b/drivers/coresight/coresight-rpm-etm.c @@ -1,4 +1,4 @@ -/* Copyright (c) 2013, The Linux Foundation. All rights reserved. +/* Copyright (c) 2013-2014, 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 @@ -19,21 +19,67 @@ #include #include #include +#include #include #include +#include "coresight-qmi.h" struct rpm_etm_drvdata { struct device *dev; struct coresight_device *csdev; + struct mutex mutex; + struct workqueue_struct *wq; + struct qmi_handle *handle; + struct work_struct work_svc_arrive; + struct work_struct work_svc_exit; + struct work_struct work_rcv_msg; + struct notifier_block nb; }; static int rpm_etm_enable(struct coresight_device *csdev) { struct rpm_etm_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent); + struct coresight_set_etm_req_msg_v01 req; + struct coresight_set_etm_resp_msg_v01 resp = { { 0, 0 } }; + struct msg_desc req_desc, resp_desc; + int ret; + + mutex_lock(&drvdata->mutex); + + req.state = CORESIGHT_ETM_STATE_ENABLED_V01; + + req_desc.msg_id = CORESIGHT_QMI_SET_ETM_REQ_V01; + req_desc.max_msg_len = CORESIGHT_QMI_SET_ETM_REQ_MAX_LEN; + req_desc.ei_array = coresight_set_etm_req_msg_v01_ei; + + resp_desc.msg_id = CORESIGHT_QMI_SET_ETM_RESP_V01; + resp_desc.max_msg_len = CORESIGHT_QMI_SET_ETM_RESP_MAX_LEN; + resp_desc.ei_array = coresight_set_etm_resp_msg_v01_ei; + + ret = qmi_send_req_wait(drvdata->handle, &req_desc, &req, sizeof(req), + &resp_desc, &resp, sizeof(resp), TIMEOUT_MS); + + if (ret < 0) { + dev_err(drvdata->dev, "%s: QMI send req failed %d\n", __func__, + ret); + goto err; + } + + if (resp.resp.result != QMI_RESULT_SUCCESS_V01) { + dev_err(drvdata->dev, "%s: QMI request failed %d %d\n", + __func__, resp.resp.result, resp.resp.error); + ret = -EREMOTEIO; + goto err; + } + + mutex_unlock(&drvdata->mutex); dev_info(drvdata->dev, "RPM ETM tracing enabled\n"); return 0; +err: + mutex_unlock(&drvdata->mutex); + return ret; } @@ -41,8 +87,43 @@ static void rpm_etm_disable(struct coresight_device *csdev) { struct rpm_etm_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent); + struct coresight_set_etm_req_msg_v01 req; + struct coresight_set_etm_resp_msg_v01 resp = { { 0, 0 } }; + struct msg_desc req_desc, resp_desc; + int ret; + + mutex_lock(&drvdata->mutex); + + req.state = CORESIGHT_ETM_STATE_DISABLED_V01; + + req_desc.msg_id = CORESIGHT_QMI_SET_ETM_REQ_V01; + req_desc.max_msg_len = CORESIGHT_QMI_SET_ETM_REQ_MAX_LEN; + req_desc.ei_array = coresight_set_etm_req_msg_v01_ei; + + resp_desc.msg_id = CORESIGHT_QMI_SET_ETM_RESP_V01; + resp_desc.max_msg_len = CORESIGHT_QMI_SET_ETM_RESP_MAX_LEN; + resp_desc.ei_array = coresight_set_etm_resp_msg_v01_ei; + + ret = qmi_send_req_wait(drvdata->handle, &req_desc, &req, sizeof(req), + &resp_desc, &resp, sizeof(resp), TIMEOUT_MS); + if (ret < 0) { + dev_err(drvdata->dev, "%s: QMI send req failed %d\n", __func__, + ret); + goto err; + } + + if (resp.resp.result != QMI_RESULT_SUCCESS_V01) { + dev_err(drvdata->dev, "%s: QMI request failed %d %d\n", + __func__, resp.resp.result, resp.resp.error); + goto err; + } + + mutex_unlock(&drvdata->mutex); dev_info(drvdata->dev, "RPM ETM tracing disabled\n"); + return; +err: + mutex_unlock(&drvdata->mutex); } static const struct coresight_ops_source rpm_etm_source_ops = { @@ -54,12 +135,89 @@ static const struct coresight_ops rpm_cs_ops = { .source_ops = &rpm_etm_source_ops, }; +static void rpm_etm_rcv_msg(struct work_struct *work) +{ + struct rpm_etm_drvdata *drvdata = container_of(work, + struct rpm_etm_drvdata, + work_rcv_msg); + + if (qmi_recv_msg(drvdata->handle) < 0) + pr_err("%s: Error receiving QMI message\n", __func__); +} + +static void rpm_etm_notify(struct qmi_handle *handle, + enum qmi_event_type event, void *notify_priv) +{ + struct rpm_etm_drvdata *drvdata = + (struct rpm_etm_drvdata *)notify_priv; + switch (event) { + case QMI_RECV_MSG: + queue_work(drvdata->wq, &drvdata->work_rcv_msg); + break; + default: + break; + } +} + +static void rpm_etm_svc_arrive(struct work_struct *work) +{ + struct rpm_etm_drvdata *drvdata = container_of(work, + struct rpm_etm_drvdata, + work_svc_arrive); + + drvdata->handle = qmi_handle_create(rpm_etm_notify, drvdata); + if (!drvdata->handle) { + pr_err("%s: QMI client handle alloc failed\n", __func__); + return; + } + + if (qmi_connect_to_service(drvdata->handle, CORESIGHT_QMI_SVC_ID, + CORESIGHT_QMI_VERSION, + CORESIGHT_SVC_INST_ID_RPM_V01) < 0) { + pr_err("%s: Could not connect handle to service\n", __func__); + qmi_handle_destroy(drvdata->handle); + drvdata->handle = NULL; + } +} + +static void rpm_etm_svc_exit(struct work_struct *work) +{ + struct rpm_etm_drvdata *drvdata = container_of(work, + struct rpm_etm_drvdata, + work_svc_exit); + + qmi_handle_destroy(drvdata->handle); + drvdata->handle = NULL; +} + +static int rpm_etm_svc_event_notify(struct notifier_block *this, + unsigned long event, + void *data) +{ + struct rpm_etm_drvdata *drvdata = container_of(this, + struct rpm_etm_drvdata, + nb); + + switch (event) { + case QMI_SERVER_ARRIVE: + queue_work(drvdata->wq, &drvdata->work_svc_arrive); + break; + case QMI_SERVER_EXIT: + queue_work(drvdata->wq, &drvdata->work_svc_exit); + break; + default: + break; + } + return 0; +} + static int rpm_etm_probe(struct platform_device *pdev) { struct device *dev = &pdev->dev; struct coresight_platform_data *pdata; struct rpm_etm_drvdata *drvdata; struct coresight_desc *desc; + int ret; if (pdev->dev.of_node) { pdata = of_get_coresight_platform_data(dev, pdev->dev.of_node); @@ -79,6 +237,23 @@ static int rpm_etm_probe(struct platform_device *pdev) if (!desc) return -ENOMEM; + mutex_init(&drvdata->mutex); + + drvdata->nb.notifier_call = rpm_etm_svc_event_notify; + + drvdata->wq = create_singlethread_workqueue("rpm-etm"); + if (!drvdata->wq) + return -EFAULT; + INIT_WORK(&drvdata->work_svc_arrive, rpm_etm_svc_arrive); + INIT_WORK(&drvdata->work_svc_exit, rpm_etm_svc_exit); + INIT_WORK(&drvdata->work_rcv_msg, rpm_etm_rcv_msg); + ret = qmi_svc_event_notifier_register(CORESIGHT_QMI_SVC_ID, + CORESIGHT_QMI_VERSION, + CORESIGHT_SVC_INST_ID_RPM_V01, + &drvdata->nb); + if (ret < 0) + goto err0; + desc->type = CORESIGHT_DEV_TYPE_SOURCE; desc->subtype.source_subtype = CORESIGHT_DEV_SUBTYPE_SOURCE_PROC; desc->ops = &rpm_cs_ops; @@ -86,11 +261,21 @@ static int rpm_etm_probe(struct platform_device *pdev) desc->dev = &pdev->dev; desc->owner = THIS_MODULE; drvdata->csdev = coresight_register(desc); - if (IS_ERR(drvdata->csdev)) - return PTR_ERR(drvdata->csdev); - + if (IS_ERR(drvdata->csdev)) { + ret = PTR_ERR(drvdata->csdev); + goto err1; + } dev_info(dev, "RPM ETM initialized\n"); return 0; +err1: + qmi_svc_event_notifier_unregister(CORESIGHT_QMI_SVC_ID, + CORESIGHT_QMI_VERSION, + CORESIGHT_SVC_INST_ID_RPM_V01, + &drvdata->nb); +err0: + destroy_workqueue(drvdata->wq); + return ret; + } static int rpm_etm_remove(struct platform_device *pdev) diff --git a/drivers/coresight/coresight-wcn-etm.c b/drivers/coresight/coresight-wcn-etm.c index 44852fb8209..b09e035d92d 100644 --- a/drivers/coresight/coresight-wcn-etm.c +++ b/drivers/coresight/coresight-wcn-etm.c @@ -1,4 +1,4 @@ -/* Copyright (c) 2013, The Linux Foundation. All rights reserved. +/* Copyright (c) 2013-2014, 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 @@ -19,21 +19,67 @@ #include #include #include +#include #include #include +#include "coresight-qmi.h" struct wcn_etm_drvdata { struct device *dev; struct coresight_device *csdev; + struct mutex mutex; + struct workqueue_struct *wq; + struct qmi_handle *handle; + struct work_struct work_svc_arrive; + struct work_struct work_svc_exit; + struct work_struct work_rcv_msg; + struct notifier_block nb; }; static int wcn_etm_enable(struct coresight_device *csdev) { struct wcn_etm_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent); + struct coresight_set_etm_req_msg_v01 req; + struct coresight_set_etm_resp_msg_v01 resp = { { 0, 0 } }; + struct msg_desc req_desc, resp_desc; + int ret; + + mutex_lock(&drvdata->mutex); + + req.state = CORESIGHT_ETM_STATE_ENABLED_V01; + + req_desc.msg_id = CORESIGHT_QMI_SET_ETM_REQ_V01; + req_desc.max_msg_len = CORESIGHT_QMI_SET_ETM_REQ_MAX_LEN; + req_desc.ei_array = coresight_set_etm_req_msg_v01_ei; + + resp_desc.msg_id = CORESIGHT_QMI_SET_ETM_RESP_V01; + resp_desc.max_msg_len = CORESIGHT_QMI_SET_ETM_RESP_MAX_LEN; + resp_desc.ei_array = coresight_set_etm_resp_msg_v01_ei; + + ret = qmi_send_req_wait(drvdata->handle, &req_desc, &req, sizeof(req), + &resp_desc, &resp, sizeof(resp), TIMEOUT_MS); + + if (ret < 0) { + dev_err(drvdata->dev, "%s: QMI send req failed %d\n", __func__, + ret); + goto err; + } + + if (resp.resp.result != QMI_RESULT_SUCCESS_V01) { + dev_err(drvdata->dev, "%s: QMI request failed %d %d\n", + __func__, resp.resp.result, resp.resp.error); + ret = -EREMOTEIO; + goto err; + } + + mutex_unlock(&drvdata->mutex); dev_info(drvdata->dev, "Wireless ETM tracing enabled\n"); return 0; +err: + mutex_unlock(&drvdata->mutex); + return ret; } @@ -41,8 +87,43 @@ static void wcn_etm_disable(struct coresight_device *csdev) { struct wcn_etm_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent); + struct coresight_set_etm_req_msg_v01 req; + struct coresight_set_etm_resp_msg_v01 resp = { { 0, 0 } }; + struct msg_desc req_desc, resp_desc; + int ret; + + mutex_lock(&drvdata->mutex); + + req.state = CORESIGHT_ETM_STATE_DISABLED_V01; + + req_desc.msg_id = CORESIGHT_QMI_SET_ETM_REQ_V01; + req_desc.max_msg_len = CORESIGHT_QMI_SET_ETM_REQ_MAX_LEN; + req_desc.ei_array = coresight_set_etm_req_msg_v01_ei; + + resp_desc.msg_id = CORESIGHT_QMI_SET_ETM_RESP_V01; + resp_desc.max_msg_len = CORESIGHT_QMI_SET_ETM_RESP_MAX_LEN; + resp_desc.ei_array = coresight_set_etm_resp_msg_v01_ei; + + ret = qmi_send_req_wait(drvdata->handle, &req_desc, &req, sizeof(req), + &resp_desc, &resp, sizeof(resp), TIMEOUT_MS); + if (ret < 0) { + dev_err(drvdata->dev, "%s: QMI send req failed %d\n", __func__, + ret); + goto err; + } + + if (resp.resp.result != QMI_RESULT_SUCCESS_V01) { + dev_err(drvdata->dev, "%s: QMI request failed %d %d\n", + __func__, resp.resp.result, resp.resp.error); + goto err; + } + + mutex_unlock(&drvdata->mutex); dev_info(drvdata->dev, "Wireless ETM tracing disabled\n"); + return; +err: + mutex_unlock(&drvdata->mutex); } static const struct coresight_ops_source wcn_etm_source_ops = { @@ -54,12 +135,89 @@ static const struct coresight_ops wcn_cs_ops = { .source_ops = &wcn_etm_source_ops, }; +static void wcn_etm_rcv_msg(struct work_struct *work) +{ + struct wcn_etm_drvdata *drvdata = container_of(work, + struct wcn_etm_drvdata, + work_rcv_msg); + + if (qmi_recv_msg(drvdata->handle) < 0) + pr_err("%s: Error receiving QMI message\n", __func__); +} + +static void wcn_etm_notify(struct qmi_handle *handle, + enum qmi_event_type event, void *notify_priv) +{ + struct wcn_etm_drvdata *drvdata = + (struct wcn_etm_drvdata *)notify_priv; + switch (event) { + case QMI_RECV_MSG: + queue_work(drvdata->wq, &drvdata->work_rcv_msg); + break; + default: + break; + } +} + +static void wcn_etm_svc_arrive(struct work_struct *work) +{ + struct wcn_etm_drvdata *drvdata = container_of(work, + struct wcn_etm_drvdata, + work_svc_arrive); + + drvdata->handle = qmi_handle_create(wcn_etm_notify, drvdata); + if (!drvdata->handle) { + pr_err("%s: QMI client handle alloc failed\n", __func__); + return; + } + + if (qmi_connect_to_service(drvdata->handle, CORESIGHT_QMI_SVC_ID, + CORESIGHT_QMI_VERSION, + CORESIGHT_SVC_INST_ID_WCN_V01) < 0) { + pr_err("%s: Could not connect handle to service\n", __func__); + qmi_handle_destroy(drvdata->handle); + drvdata->handle = NULL; + } +} + +static void wcn_etm_svc_exit(struct work_struct *work) +{ + struct wcn_etm_drvdata *drvdata = container_of(work, + struct wcn_etm_drvdata, + work_svc_exit); + + qmi_handle_destroy(drvdata->handle); + drvdata->handle = NULL; +} + +static int wcn_etm_svc_event_notify(struct notifier_block *this, + unsigned long event, + void *data) +{ + struct wcn_etm_drvdata *drvdata = container_of(this, + struct wcn_etm_drvdata, + nb); + + switch (event) { + case QMI_SERVER_ARRIVE: + queue_work(drvdata->wq, &drvdata->work_svc_arrive); + break; + case QMI_SERVER_EXIT: + queue_work(drvdata->wq, &drvdata->work_svc_exit); + break; + default: + break; + } + return 0; +} + static int wcn_etm_probe(struct platform_device *pdev) { struct device *dev = &pdev->dev; struct coresight_platform_data *pdata; struct wcn_etm_drvdata *drvdata; struct coresight_desc *desc; + int ret; if (pdev->dev.of_node) { pdata = of_get_coresight_platform_data(dev, pdev->dev.of_node); @@ -79,6 +237,23 @@ static int wcn_etm_probe(struct platform_device *pdev) if (!desc) return -ENOMEM; + mutex_init(&drvdata->mutex); + + drvdata->nb.notifier_call = wcn_etm_svc_event_notify; + + drvdata->wq = create_singlethread_workqueue("wcn-etm"); + if (!drvdata->wq) + return -EFAULT; + INIT_WORK(&drvdata->work_svc_arrive, wcn_etm_svc_arrive); + INIT_WORK(&drvdata->work_svc_exit, wcn_etm_svc_exit); + INIT_WORK(&drvdata->work_rcv_msg, wcn_etm_rcv_msg); + ret = qmi_svc_event_notifier_register(CORESIGHT_QMI_SVC_ID, + CORESIGHT_QMI_VERSION, + CORESIGHT_SVC_INST_ID_WCN_V01, + &drvdata->nb); + if (ret < 0) + goto err0; + desc->type = CORESIGHT_DEV_TYPE_SOURCE; desc->subtype.source_subtype = CORESIGHT_DEV_SUBTYPE_SOURCE_PROC; desc->ops = &wcn_cs_ops; @@ -86,11 +261,22 @@ static int wcn_etm_probe(struct platform_device *pdev) desc->dev = &pdev->dev; desc->owner = THIS_MODULE; drvdata->csdev = coresight_register(desc); - if (IS_ERR(drvdata->csdev)) - return PTR_ERR(drvdata->csdev); + if (IS_ERR(drvdata->csdev)) { + ret = PTR_ERR(drvdata->csdev); + goto err1; + } dev_info(dev, "Wireless ETM initialized\n"); return 0; +err1: + qmi_svc_event_notifier_unregister(CORESIGHT_QMI_SVC_ID, + CORESIGHT_QMI_VERSION, + CORESIGHT_SVC_INST_ID_WCN_V01, + &drvdata->nb); +err0: + destroy_workqueue(drvdata->wq); + return ret; + } static int wcn_etm_remove(struct platform_device *pdev)