diff --git a/vm/fe/src/mmrm_vm_fe.h b/vm/fe/src/mmrm_vm_fe.h index 74a6329af7..b728b93830 100644 --- a/vm/fe/src/mmrm_vm_fe.h +++ b/vm/fe/src/mmrm_vm_fe.h @@ -9,14 +9,16 @@ #include #include -struct mmrm_vm_fe_clk_src_info { - struct mmrm_clk_client_desc client_desc; - int flags; +#define MMRM_SYSFS_ENTRY_MAX_LEN PAGE_SIZE + +struct mmrm_vm_fe_clk_client_desc { + u32 client_domain; + u32 client_id; + u32 num_hw_blocks; }; - struct mmrm_vm_fe_clk_src_set { - struct mmrm_vm_fe_clk_src_info *clk_src_tbl; + struct mmrm_vm_fe_clk_client_desc *clk_src_tbl; u32 count; }; @@ -25,12 +27,19 @@ struct mmrm_vm_fe_priv { struct mmrm_client *client_tbl; - struct list_head mmrm_work_list; - struct mutex work_list_lock; + struct list_head resp_works; + struct mutex resp_works_lock; struct mmrm_vm_fe_clk_src_set clk_src_set; struct mutex msg_send_lock; int seq_no; + bool is_clk_scaling_supported; +}; + +struct mmrm_vm_fe_pkt { + struct mmrm_vm_msg_q msgq; + struct mmrm_vm_request_msg_pkt req_pkt; + struct mmrm_vm_response_msg_pkt resp_pkt; }; /* @@ -63,6 +72,28 @@ struct mmrm_client *mmrm_vm_fe_get_client(u32 client_id); */ int mmrm_vm_fe_load_clk_rsrc(struct mmrm_vm_driver_data *drv_priv); +/* + * mmrm_vm_fe_clk_src_check -- check if fe support the clk src + * @desc: clk src description + */ +struct mmrm_vm_fe_clk_client_desc *mmrm_vm_fe_clk_src_get(struct mmrm_client_desc *desc); + +/* + * init_lookup_table -- init et clk lookup table + * @mmrm_vm: device data + */ +int mmrm_vm_fe_init_lookup_table(struct mmrm_vm_driver_data *mmrm_vm); + +/* + * mmrm_vm_fe_clk_print_info -- output clk info through sys + * @clk_src_set: clk info + * @buf: received output buffer + * @max_len: buffer length + */ +int mmrm_vm_fe_clk_print_info( + struct mmrm_vm_fe_clk_src_set *clk_src_set, + char *buf, int max_len); + /* * mmrm_vm_fe_recv -- process received response info * @mmrm_vm: device data @@ -71,6 +102,12 @@ int mmrm_vm_fe_load_clk_rsrc(struct mmrm_vm_driver_data *drv_priv); */ void mmrm_vm_fe_recv(struct mmrm_vm_driver_data *mmrm_vm, void *data, size_t size); +/* + * mmrm_vm_fe_count_clk_clients_frm_dt -- process received response info + * @pdev: platform device + */ +int mmrm_vm_fe_count_clk_clients_frm_dt(struct platform_device *pdev); + #endif /* __MMRM_VM_FE_H__ */ diff --git a/vm/fe/src/mmrm_vm_fe_api.c b/vm/fe/src/mmrm_vm_fe_api.c index 826fb8532f..39349d85fe 100644 --- a/vm/fe/src/mmrm_vm_fe_api.c +++ b/vm/fe/src/mmrm_vm_fe_api.c @@ -17,6 +17,7 @@ #include "mmrm_vm_fe.h" #include "mmrm_vm_interface.h" #include "mmrm_vm_msgq.h" +#include "mmrm_vm_debug.h" #define get_client_handle_2_id(client) (client->client_uid) @@ -24,6 +25,14 @@ extern struct mmrm_vm_driver_data *drv_vm_fe; #define MAX_TIMEOUT_MS 300 +#define CHECK_SKIP_MMRM_CLK_RSRC(drv_data) \ +{ \ + if (!drv_data->is_clk_scaling_supported) { \ + d_mpr_h("%s: mmrm clk rsrc not supported\n", __func__);\ + goto skip_mmrm; \ + } \ +} + int mmrm_fe_append_work_list(struct mmrm_vm_msg_q *msg_q, int msg_sz) { struct mmrm_vm_request_msg_pkt *msg_pkt = msg_q->m_req; @@ -31,71 +40,128 @@ int mmrm_fe_append_work_list(struct mmrm_vm_msg_q *msg_q, int msg_sz) unsigned long waited_time_ms; init_completion(&msg_q->complete); - mutex_lock(&fe_data->work_list_lock); - list_add_tail(&msg_q->link, &fe_data->mmrm_work_list); - mutex_unlock(&fe_data->work_list_lock); + mutex_lock(&fe_data->resp_works_lock); + list_add_tail(&msg_q->link, &fe_data->resp_works); + mutex_unlock(&fe_data->resp_works_lock); mutex_lock(&fe_data->msg_send_lock); msg_pkt->msg.hd.seq_no = fe_data->seq_no++; mutex_unlock(&fe_data->msg_send_lock); + d_mpr_w("%s: seq no:%d\n", __func__, msg_pkt->msg.hd.seq_no); + mmrm_vm_fe_request_send(drv_vm_fe, msg_pkt, msg_sz); waited_time_ms = wait_for_completion_timeout(&msg_q->complete, msecs_to_jiffies(MAX_TIMEOUT_MS)); - if (waited_time_ms >= MAX_TIMEOUT_MS) + if (waited_time_ms >= MAX_TIMEOUT_MS) { + d_mpr_e("%s: request send timeout\n", __func__); return -1; + } return 0; } +struct mmrm_vm_msg_q *get_msg_work(void) +{ + struct mmrm_vm_msg_q *msg_q; + struct mmrm_vm_fe_pkt *data; + + data = kzalloc(sizeof(struct mmrm_vm_fe_pkt), GFP_KERNEL); + if (data == NULL) + goto err_mem_fail; + + msg_q = &data->msgq; + msg_q->m_req = &data->req_pkt; + msg_q->m_resp = &data->resp_pkt; + + return msg_q; + +err_mem_fail: + d_mpr_e("%s: failed to alloc msg buffer\n", __func__); + return NULL; +} + +void release_msg_work(struct mmrm_vm_msg_q *msg_q) +{ + struct mmrm_vm_fe_pkt *data; + + if (msg_q == NULL) { + d_mpr_e("%s: release null msg ptr\n", __func__); + return; + } + data = container_of(msg_q, struct mmrm_vm_fe_pkt, msgq); + kfree(data); +} + struct mmrm_client *mmrm_client_register(struct mmrm_client_desc *desc) { - struct mmrm_vm_request_msg_pkt msg; - struct mmrm_vm_response_msg_pkt resp_pkt; - struct mmrm_vm_api_request_msg *api_msg = &msg.msg; - struct mmrm_vm_register_request *reg_data = &api_msg->data.reg; + struct mmrm_vm_msg_q *msg_q; + struct mmrm_vm_api_request_msg *api_msg; + struct mmrm_vm_register_request *reg_data; size_t msg_size = sizeof(api_msg->hd) + sizeof(*reg_data); int rc = 0; + struct mmrm_client *client = NULL; - struct mmrm_vm_msg_q msg_q; + if (mmrm_vm_fe_clk_src_get(desc) == NULL) { + d_mpr_e("%s: FE doesn't support clk domain=%d client id=%d\n", __func__, + desc->client_info.desc.client_domain, desc->client_info.desc.client_id); + goto err_clk_src; + } + + msg_q = get_msg_work(); + if (msg_q == NULL) { + d_mpr_e("%s: failed to alloc msg buf\n", __func__); + goto err_no_mem; + } + api_msg = &msg_q->m_req->msg; + reg_data = &api_msg->data.reg; api_msg->hd.cmd_id = MMRM_VM_REQUEST_REGISTER; reg_data->client_type = desc->client_type; reg_data->priority = desc->priority; memcpy(®_data->desc, &desc->client_info.desc, sizeof(reg_data->desc)); - msg_q.m_req = &msg; - msg_q.m_resp = &resp_pkt; - rc = mmrm_fe_append_work_list(&msg_q, msg_size); - if (rc != 0) - return NULL; + rc = mmrm_fe_append_work_list(msg_q, msg_size); + if (rc == 0) { + client = mmrm_vm_fe_get_client(msg_q->m_resp->msg.data.reg.client_id); + }; - return mmrm_vm_fe_get_client(resp_pkt.msg.data.reg.client_id); + release_msg_work(msg_q); + +err_no_mem: +err_clk_src: + return client; } EXPORT_SYMBOL(mmrm_client_register); int mmrm_client_deregister(struct mmrm_client *client) { - int rc = 0; - struct mmrm_vm_request_msg_pkt msg; - struct mmrm_vm_response_msg_pkt resp_pkt; - struct mmrm_vm_api_request_msg *api_msg = &msg.msg; - struct mmrm_vm_deregister_request *reg_data = &api_msg->data.dereg; + int rc = -1; + struct mmrm_vm_api_request_msg *api_msg; + struct mmrm_vm_deregister_request *reg_data; + struct mmrm_vm_msg_q *msg_q; + size_t msg_size = sizeof(api_msg->hd) + sizeof(*reg_data); - struct mmrm_vm_msg_q msg_q; + msg_q = get_msg_work(); + if (msg_q == NULL) { + d_mpr_e("%s: failed to alloc msg buf\n", __func__); + goto err_no_mem; + } + api_msg = &msg_q->m_req->msg; + reg_data = &api_msg->data.dereg; api_msg->hd.cmd_id = MMRM_VM_REQUEST_DEREGISTER; reg_data->client_id = get_client_handle_2_id(client); - msg_q.m_req = &msg; - msg_q.m_resp = &resp_pkt; - rc = mmrm_fe_append_work_list(&msg_q, msg_size); - if (rc != 0) - return rc; + rc = mmrm_fe_append_work_list(msg_q, msg_size); + if (rc == 0) + rc = msg_q->m_resp->msg.data.dereg.ret_code; - rc = resp_pkt.msg.data.dereg.ret_code; + release_msg_work(msg_q); + +err_no_mem: return rc; } EXPORT_SYMBOL(mmrm_client_deregister); @@ -103,14 +169,20 @@ EXPORT_SYMBOL(mmrm_client_deregister); int mmrm_client_set_value(struct mmrm_client *client, struct mmrm_client_data *client_data, unsigned long val) { - int rc = 0; - struct mmrm_vm_request_msg_pkt msg; - struct mmrm_vm_response_msg_pkt resp_pkt; - struct mmrm_vm_api_request_msg *api_msg = &msg.msg; - struct mmrm_vm_setvalue_request *reg_data = &api_msg->data.setval; + int rc = -1; + struct mmrm_vm_api_request_msg *api_msg; + struct mmrm_vm_setvalue_request *reg_data; + struct mmrm_vm_msg_q *msg_q; + size_t msg_size = sizeof(api_msg->hd) + sizeof(*reg_data); - struct mmrm_vm_msg_q msg_q; + msg_q = get_msg_work(); + if (msg_q == NULL) { + d_mpr_e("%s: failed to alloc msg buf\n", __func__); + goto err_no_mem; + } + api_msg = &msg_q->m_req->msg; + reg_data = &api_msg->data.setval; api_msg->hd.cmd_id = MMRM_VM_REQUEST_SETVALUE; reg_data->client_id = get_client_handle_2_id(client); @@ -118,15 +190,14 @@ int mmrm_client_set_value(struct mmrm_client *client, reg_data->data.num_hw_blocks = client_data->num_hw_blocks; reg_data->val = val; - msg_q.m_req = &msg; - msg_q.m_resp = &resp_pkt; - - rc = mmrm_fe_append_work_list(&msg_q, msg_size); + rc = mmrm_fe_append_work_list(msg_q, msg_size); if (rc != 0) return rc; - rc = resp_pkt.msg.data.setval.val; + rc = msg_q->m_resp->msg.data.setval.val; + d_mpr_h("%s: done rc=%d\n", __func__, rc); +err_no_mem: return rc; } EXPORT_SYMBOL(mmrm_client_set_value); @@ -135,14 +206,19 @@ int mmrm_client_set_value_in_range(struct mmrm_client *client, struct mmrm_client_data *client_data, struct mmrm_client_res_value *val) { - int rc = 0; - struct mmrm_vm_request_msg_pkt msg; - struct mmrm_vm_response_msg_pkt resp_pkt; - struct mmrm_vm_api_request_msg *api_msg = &msg.msg; - struct mmrm_vm_setvalue_inrange_request *reg_data = &api_msg->data.setval_range; + int rc = -1; + struct mmrm_vm_api_request_msg *api_msg ; + struct mmrm_vm_setvalue_inrange_request *reg_data; size_t msg_size = sizeof(api_msg->hd) + sizeof(*reg_data); + struct mmrm_vm_msg_q *msg_q; - struct mmrm_vm_msg_q msg_q; + msg_q = get_msg_work(); + if (msg_q == NULL) { + d_mpr_e("%s: failed to alloc msg buf\n", __func__); + goto err_no_mem; + } + api_msg = &msg_q->m_req->msg; + reg_data = &api_msg->data.setval_range; api_msg->hd.cmd_id = MMRM_VM_REQUEST_SETVALUE_INRANGE; reg_data->client_id = get_client_handle_2_id(client); @@ -152,10 +228,9 @@ int mmrm_client_set_value_in_range(struct mmrm_client *client, reg_data->val.max = val->max; reg_data->val.min = val->min; - msg_q.m_req = &msg; - msg_q.m_resp = &resp_pkt; + rc = mmrm_fe_append_work_list(msg_q, msg_size); - rc = mmrm_fe_append_work_list(&msg_q, msg_size); +err_no_mem: return rc; } EXPORT_SYMBOL(mmrm_client_set_value_in_range); @@ -164,28 +239,55 @@ EXPORT_SYMBOL(mmrm_client_set_value_in_range); int mmrm_client_get_value(struct mmrm_client *client, struct mmrm_client_res_value *val) { - int rc = 0; - struct mmrm_vm_request_msg_pkt msg; - struct mmrm_vm_response_msg_pkt resp_pkt; - struct mmrm_vm_api_request_msg *api_msg = &msg.msg; - struct mmrm_vm_getvalue_request *reg_data = &api_msg->data.getval; + int rc = -1; + struct mmrm_vm_api_request_msg *api_msg; + struct mmrm_vm_getvalue_request *reg_data; size_t msg_size = sizeof(api_msg->hd) + sizeof(*reg_data); + struct mmrm_vm_msg_q *msg_q; - struct mmrm_vm_msg_q msg_q; + msg_q = get_msg_work(); + if (msg_q == NULL) { + d_mpr_e("%s: failed to alloc msg buf\n", __func__); + goto err_no_mem; + } + api_msg = &msg_q->m_req->msg; + reg_data = &api_msg->data.getval; api_msg->hd.cmd_id = MMRM_VM_REQUEST_GETVALUE; reg_data->client_id = get_client_handle_2_id(client); - msg_q.m_req = &msg; - msg_q.m_resp = &resp_pkt; - rc = mmrm_fe_append_work_list(&msg_q, msg_size); + rc = mmrm_fe_append_work_list(msg_q, msg_size); if (rc == 0) { - val->cur = resp_pkt.msg.data.getval.val.cur; - val->max = resp_pkt.msg.data.getval.val.max; - val->min = resp_pkt.msg.data.getval.val.min; + val->cur = msg_q->m_resp->msg.data.getval.val.cur; + val->max = msg_q->m_resp->msg.data.getval.val.max; + val->min = msg_q->m_resp->msg.data.getval.val.min; } + +err_no_mem: return rc; } EXPORT_SYMBOL(mmrm_client_get_value); + +bool mmrm_client_check_scaling_supported(enum mmrm_client_type client_type, u32 client_domain) +{ + struct mmrm_vm_fe_priv *fe_data; + + if (drv_vm_fe == (void *)-EPROBE_DEFER) { + d_mpr_e("%s: mmrm probe_init not done\n", __func__); + goto err_exit; + } + + fe_data = drv_vm_fe->vm_pvt_data; + if (client_type == MMRM_CLIENT_CLOCK) { + CHECK_SKIP_MMRM_CLK_RSRC(fe_data); + } + + return true; +err_exit: + d_mpr_e("%s: error exit\n", __func__); +skip_mmrm: + return false; +} +EXPORT_SYMBOL(mmrm_client_check_scaling_supported); diff --git a/vm/fe/src/mmrm_vm_fe_frontend.c b/vm/fe/src/mmrm_vm_fe_frontend.c index cdc70ae802..5ef249bf3b 100644 --- a/vm/fe/src/mmrm_vm_fe_frontend.c +++ b/vm/fe/src/mmrm_vm_fe_frontend.c @@ -4,6 +4,7 @@ */ #include +#include #include "mmrm_vm_fe.h" #include "mmrm_vm_interface.h" @@ -20,24 +21,26 @@ void mmrm_vm_fe_recv(struct mmrm_vm_driver_data *mmrm_vm, void *data, size_t siz u64 kt2; struct mmrm_vm_fe_priv *fe_data = mmrm_vm->vm_pvt_data; - mutex_lock(&fe_data->work_list_lock); - list_for_each_entry_safe(node, temp, &fe_data->mmrm_work_list, link) { + mutex_lock(&fe_data->resp_works_lock); + list_for_each_entry_safe(node, temp, &fe_data->resp_works, link) { if (msg->hd.seq_no == node->m_req->msg.hd.seq_no) { - d_mpr_e("%s: seq no:%d\n", __func__, msg->hd.seq_no); + d_mpr_w("%s: seq no:%d\n", __func__, msg->hd.seq_no); list_del(&node->link); rc = 0; break; } } - mutex_unlock(&fe_data->work_list_lock); - if (rc != 0) + mutex_unlock(&fe_data->resp_works_lock); + if (rc != 0) { + d_mpr_e("%s: seq no:%d wrong\n", __func__, msg->hd.seq_no); return; + } - d_mpr_e("%s: cmd:%d\n", __func__, msg->hd.cmd_id); + d_mpr_w("%s: cmd:%d\n", __func__, msg->hd.cmd_id); switch (msg->hd.cmd_id) { case MMRM_VM_RESPONSE_REGISTER: node->m_resp->msg.data.reg.client_id = msg->data.reg.client_id; - d_mpr_h("%s: client:%d\n", __func__, msg->data.reg.client_id); + d_mpr_e("%s: client_id:%u\n", __func__, msg->data.reg.client_id); break; case MMRM_VM_RESPONSE_SETVALUE: node->m_resp->msg.data.setval.val = msg->data.setval.val; @@ -85,22 +88,31 @@ int mmrm_vm_fe_request_send(struct mmrm_vm_driver_data *mmrm_vm, return -E2BIG; } - d_mpr_h("%s: handle=%p\n", __func__, pmsg_info->msgq_handle); rc = gh_msgq_send(pmsg_info->msgq_handle, msg_pkt, - msg_size + sizeof(msg_pkt->hdr), GH_MSGQ_TX_PUSH); - - d_mpr_h("%s: handle=%p result:%d\n", __func__, pmsg_info->msgq_handle, rc); + msg_size + sizeof(msg_pkt->hdr), GH_MSGQ_TX_PUSH); return rc; } +int mmrm_vm_fe_count_clk_clients_frm_dt(struct platform_device *pdev) +{ + u32 size_clk_src = 0, num_clk_src = 0; + + of_find_property(pdev->dev.of_node, "mmrm-client-info", &size_clk_src); + num_clk_src = size_clk_src / sizeof(struct mmrm_vm_fe_clk_client_desc); + d_mpr_h("%s: found %d clk_srcs size %d\n", + __func__, num_clk_src, size_clk_src); + + return num_clk_src; +} + int mmrm_vm_fe_load_clk_rsrc(struct mmrm_vm_driver_data *mmrm_vm) { int rc = 0, num_clk_src = 0; - int c = 0, size_clk_src = 0, entry_offset = 5; + int c = 0, size_clk_src = 0, entry_offset = 3; struct platform_device *pdev; - struct mmrm_vm_fe_clk_src_info *pclk_src; + struct mmrm_vm_fe_clk_client_desc *pclk_src; struct mmrm_vm_fe_priv *fe_data = mmrm_vm->vm_pvt_data; pdev = container_of(fe_data->dev, struct platform_device, dev); @@ -123,10 +135,10 @@ int mmrm_vm_fe_load_clk_rsrc(struct mmrm_vm_driver_data *mmrm_vm) rc = -ENOMEM; goto err_load_clk_src_tbl; } - num_clk_src = size_clk_src / sizeof(struct mmrm_clk_client_desc); + num_clk_src = size_clk_src / sizeof(struct mmrm_vm_fe_clk_client_desc); fe_data->clk_src_set.count = num_clk_src; - d_mpr_h("%s: found %d clk_srcs size %d\n", + d_mpr_w("%s: found %d clk_srcs size %d\n", __func__, num_clk_src, size_clk_src); for (c = 0; c < num_clk_src; c++) { @@ -134,10 +146,13 @@ int mmrm_vm_fe_load_clk_rsrc(struct mmrm_vm_driver_data *mmrm_vm) of_property_read_u32_index(pdev->dev.of_node, "mmrm-client-info", (c*entry_offset), - &pclk_src->client_desc.client_domain); + &pclk_src->client_domain); of_property_read_u32_index(pdev->dev.of_node, "mmrm-client-info", (c*entry_offset+1), - &pclk_src->client_desc.client_id); + &pclk_src->client_id); + of_property_read_u32_index(pdev->dev.of_node, + "mmrm-client-info", (c*entry_offset+2), + &pclk_src->num_hw_blocks); } return 0; @@ -146,24 +161,68 @@ err_load_clk_src_tbl: return rc; } -void init_lookup_table(struct mmrm_vm_driver_data *mmrm_vm) +struct mmrm_vm_fe_clk_client_desc *mmrm_vm_fe_clk_src_get(struct mmrm_client_desc *desc) { - int i; + struct mmrm_vm_fe_priv *fe_data = drv_vm_fe->vm_pvt_data; + int num_clk_src = fe_data->clk_src_set.count; + struct mmrm_vm_fe_clk_client_desc *pclk_src; + int i; + + d_mpr_l("%s: num clk src=%d domain:%d id:%d\n", __func__, num_clk_src, + desc->client_info.desc.client_domain, desc->client_info.desc.client_id); + + pclk_src = fe_data->clk_src_set.clk_src_tbl; + for (i = 0; i < num_clk_src; i++, pclk_src++) { + if (pclk_src->client_domain == desc->client_info.desc.client_domain || + pclk_src->client_id == desc->client_info.desc.client_id) { + return pclk_src; + } + } + return NULL; +} + +int mmrm_vm_fe_init_lookup_table(struct mmrm_vm_driver_data *mmrm_vm) +{ + int i, rc = -1; struct platform_device *pdev; struct mmrm_vm_fe_priv *fe_data = mmrm_vm->vm_pvt_data; pdev = container_of(fe_data->dev, struct platform_device, dev); - mmrm_vm_fe_load_clk_rsrc(mmrm_vm); fe_data->client_tbl = devm_kzalloc(&pdev->dev, - fe_data->clk_src_set.count * sizeof(struct mmrm_client), GFP_KERNEL); + fe_data->clk_src_set.count * sizeof(struct mmrm_client), GFP_KERNEL); if (!fe_data->client_tbl) - return; + return rc; for (i = 0; i < fe_data->clk_src_set.count; i++) { fe_data->client_tbl[i].client_type = 0; - fe_data->client_tbl[i].client_uid = 0; + fe_data->client_tbl[i].client_uid = U32_MAX; } + return 0; +} + +int mmrm_vm_fe_clk_print_info( + struct mmrm_vm_fe_clk_src_set *clk_src_set, + char *buf, int max_len) +{ + int left_spaces = max_len; + int len, c; + struct mmrm_vm_fe_clk_client_desc *pclk_src; + int num_clk_src = clk_src_set->count; + + len = scnprintf(buf, left_spaces, "Domain ID Num\n"); + left_spaces -= len; + buf += len; + + pclk_src = clk_src_set->clk_src_tbl; + for (c = 0; c < num_clk_src; c++, pclk_src++) { + len = scnprintf(buf, left_spaces, "%d\t%d\t%d\n", + pclk_src->client_domain, pclk_src->client_id, pclk_src->num_hw_blocks); + left_spaces -= len; + buf += len; + } + + return max_len - left_spaces; } struct mmrm_client *mmrm_vm_fe_get_client(u32 client_id) @@ -172,14 +231,17 @@ struct mmrm_client *mmrm_vm_fe_get_client(u32 client_id) struct mmrm_client *ptr; struct mmrm_vm_fe_priv *fe_data = drv_vm_fe->vm_pvt_data; + if (client_id == U32_MAX) + return NULL; + for (i = 0, ptr = fe_data->client_tbl; i < fe_data->clk_src_set.count; i++, ptr++) { - if (ptr->client_uid == client_id+1) + if (ptr->client_uid == client_id) return ptr; } for (i = 0, ptr = fe_data->client_tbl; i < fe_data->clk_src_set.count; i++, ptr++) { - if (ptr->client_uid == 0) { - ptr->client_uid = client_id + 1; + if (ptr->client_uid == U32_MAX) { + ptr->client_uid = client_id; return ptr; } } diff --git a/vm/fe/src/mmrm_vm_fe_main.c b/vm/fe/src/mmrm_vm_fe_main.c index edcab4f868..320d896718 100644 --- a/vm/fe/src/mmrm_vm_fe_main.c +++ b/vm/fe/src/mmrm_vm_fe_main.c @@ -16,15 +16,27 @@ struct mmrm_vm_driver_data *drv_vm_fe = (void *) -EPROBE_DEFER; +static ssize_t dump_clk_info_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int rc; + + struct mmrm_vm_fe_priv *fe_data = drv_vm_fe->vm_pvt_data; + + rc = mmrm_vm_fe_clk_print_info(&fe_data->clk_src_set, buf, MMRM_SYSFS_ENTRY_MAX_LEN); + if (rc == 0) + d_mpr_e("%s: failed to dump clk info\n", __func__); + + return rc; +} + ssize_t msgq_send_trigger_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) { struct mmrm_vm_driver_data *priv = dev->driver_data; char send_buf[64] = "test msg"; - char recv_buf[64]; int ret; bool flag; - size_t recv_size; ret = strtobool(buf, &flag); if (ret) { @@ -32,24 +44,20 @@ ssize_t msgq_send_trigger_store(struct device *dev, struct device_attribute *att return -1; } if (flag) { - ret = gh_msgq_send(priv->msg_info.msgq_handle, send_buf, sizeof(send_buf), 0); + ret = mmrm_vm_msgq_send(priv, send_buf, sizeof(send_buf)); if (ret) - dev_err(dev, "send msgq failed\n"); + d_mpr_e("%s:send msgq failed\n", __func__); else - dev_info(dev, "send msgq success\n"); - ret = gh_msgq_recv(priv->msg_info.msgq_handle, recv_buf, sizeof(recv_buf), - &recv_size, 0); - if (ret) - dev_err(dev, "recv msgq failed ret = %d\n", ret); - else - dev_info(dev, "recv msg: %s\n", recv_buf); + d_mpr_e("%s:send msgq success\n", __func__); } return ret ? ret : count; } +static DEVICE_ATTR_RO(dump_clk_info); static DEVICE_ATTR_WO(msgq_send_trigger); static struct attribute *mmrm_vm_fe_fs_attrs[] = { + &dev_attr_dump_clk_info.attr, &dev_attr_msgq_send_trigger.attr, NULL, }; @@ -63,6 +71,7 @@ static int mmrm_vm_fe_driver_probe(struct platform_device *pdev) struct device *dev = &pdev->dev; struct mmrm_vm_fe_priv *fe_priv_data; int rc = 0; + u32 clk_clients = 0; drv_vm_fe = devm_kzalloc(dev, sizeof(*drv_vm_fe), GFP_KERNEL); if (!drv_vm_fe) @@ -78,23 +87,56 @@ static int mmrm_vm_fe_driver_probe(struct platform_device *pdev) fe_priv_data->seq_no = 0; fe_priv_data->dev = dev; + /* check for clk clients needing admission control */ + clk_clients = mmrm_vm_fe_count_clk_clients_frm_dt(pdev); + if (clk_clients) { + d_mpr_h("%s: %d clk clients managed for admission control\n", + __func__, clk_clients); + fe_priv_data->is_clk_scaling_supported = true; + } else { + d_mpr_h("%s: no clk clients managed for admission control\n", + __func__); + fe_priv_data->is_clk_scaling_supported = false; + goto skip_mmrm; + } + mutex_init(&fe_priv_data->msg_send_lock); dev_set_drvdata(&pdev->dev, drv_vm_fe); - INIT_LIST_HEAD(&fe_priv_data->mmrm_work_list); - mutex_init(&fe_priv_data->work_list_lock); + INIT_LIST_HEAD(&fe_priv_data->resp_works); + mutex_init(&fe_priv_data->resp_works_lock); mmrm_vm_fe_load_clk_rsrc(drv_vm_fe); - mmrm_vm_msgq_init(drv_vm_fe); + rc = mmrm_vm_msgq_init(drv_vm_fe); + if (rc != 0) { + d_mpr_e("%s: failed to msgq init\n", + __func__); + goto err_msgq_init; + } + + rc = mmrm_vm_fe_init_lookup_table(drv_vm_fe); + if (rc == -1) { + d_mpr_e("%s: failed to lookup table init\n", + __func__); + goto err_lookup_table; + } if (sysfs_create_group(&pdev->dev.kobj, &mmrm_vm_fe_fs_attrs_group)) { d_mpr_e("%s: failed to create sysfs\n", __func__); } - dev_err(dev, "msgq probe success"); -err_priv_data: + d_mpr_w("msgq probe success"); + return rc; +err_lookup_table: + mmrm_vm_msgq_deinit(drv_vm_fe); + +err_msgq_init: +err_priv_data: + d_mpr_e("%s: failed to probe\n", __func__); + +skip_mmrm: return rc; } diff --git a/vm/fe/src/mmrm_vm_fe_msgq.c b/vm/fe/src/mmrm_vm_fe_msgq.c index 00488aac40..79e0a964da 100644 --- a/vm/fe/src/mmrm_vm_fe_msgq.c +++ b/vm/fe/src/mmrm_vm_fe_msgq.c @@ -20,15 +20,17 @@ void mmrm_vm_fe_msgq_msg_handler(struct work_struct *work) struct mmrm_vm_driver_data *mmrm_vm = container_of(pthread_info, struct mmrm_vm_driver_data, thread_info); struct list_head head; - struct mmrm_vm_msg *next_msg; + struct mmrm_vm_msg *dummy = NULL; struct mmrm_vm_msg *msg; + struct mmrm_vm_request_msg_pkt *msg_pkt; mutex_lock(&pthread_info->list_lock); list_replace_init(&pthread_info->queued_msg, &head); mutex_unlock(&pthread_info->list_lock); - list_for_each_entry_safe(msg, next_msg, &head, link) { - mmrm_vm_fe_recv(mmrm_vm, msg->msg_buf, msg->msg_size); + list_for_each_entry_safe(msg, dummy, &head, link) { + msg_pkt = (struct mmrm_vm_request_msg_pkt *)msg->msg_buf; + mmrm_vm_fe_recv(mmrm_vm, &msg_pkt->msg, msg_pkt->hdr.size); list_del(&msg->link); kfree(msg); } @@ -51,6 +53,7 @@ int mmrm_vm_msgq_listener(void *data) ret = gh_msgq_recv(pmsg_info->msgq_handle, msg->msg_buf, GH_MSGQ_MAX_MSG_SIZE_BYTES, &size, 0); + d_mpr_l("done ret=%d\n", ret); if (ret < 0) { kfree(msg); @@ -59,7 +62,8 @@ int mmrm_vm_msgq_listener(void *data) } msg->msg_size = size; - list_add_tail(&pthread_info->queued_msg, &msg->link); + mutex_lock(&pthread_info->list_lock); + list_add_tail(&msg->link, &pthread_info->queued_msg); mutex_unlock(&pthread_info->list_lock); queue_delayed_work(pthread_info->msg_workq, @@ -90,7 +94,7 @@ int mmrm_vm_msgq_send(struct mmrm_vm_driver_data *mmrm_vm, void *msg, size_t msg } rc = gh_msgq_send(pmsg_info->msgq_handle, msg, msg_size, 0); - d_mpr_e("%s: handle=%p result:%d\n", __func__, pmsg_info->msgq_handle, rc); + d_mpr_h("%s: result:%d\n", __func__, rc); return rc; } @@ -101,14 +105,23 @@ int mmrm_vm_msgq_init(struct mmrm_vm_driver_data *mmrm_vm) struct mmrm_vm_gh_msgq_info *pmsg_info; struct mmrm_vm_thread_info *pthread_info; - if (IS_ERR_OR_NULL(mmrm_vm)) - return -EINVAL; - + if (IS_ERR_OR_NULL(mmrm_vm)) { + rc = -EINVAL; + goto err; + } pmsg_info = &mmrm_vm->msg_info; pthread_info = &mmrm_vm->thread_info; - pthread_info->msg_workq = create_singlethread_workqueue("vm_be_message_workq"); + mutex_init(&pthread_info->list_lock); + INIT_LIST_HEAD(&pthread_info->queued_msg); + + pthread_info->msg_workq = create_singlethread_workqueue("vm_fe_message_workq"); + if (IS_ERR_OR_NULL(pthread_info->msg_workq)) { + rc = -1; + goto err; + } + INIT_DELAYED_WORK(&pthread_info->msgq_work, mmrm_vm_fe_msgq_msg_handler); pmsg_info->msgq_label = GH_MSGQ_LABEL_MMRM; @@ -119,16 +132,30 @@ int mmrm_vm_msgq_init(struct mmrm_vm_driver_data *mmrm_vm) if (IS_ERR(pmsg_info->msgq_handle)) { rc = PTR_ERR(pmsg_info->msgq_handle); d_mpr_e("msgq register failed rc:%d\n", rc); - return rc; + goto err_msgq_reg; } pthread_info->msgq_listener_thread = kthread_create(mmrm_vm_msgq_listener, mmrm_vm, "mmrm_vm_fe"); + if (IS_ERR_OR_NULL(pthread_info->msgq_listener_thread)) { + rc = PTR_ERR(pmsg_info->msgq_handle); + goto err_listener_thread; + } + wake_up_process(pthread_info->msgq_listener_thread); - d_mpr_e("%s: msgq_handle=%p\n", __func__, pmsg_info->msgq_handle); + d_mpr_w("%s: msgq init done\n", __func__); return rc; + +err_listener_thread: + gh_msgq_unregister(pmsg_info->msgq_handle); + pmsg_info->msgq_handle = NULL; +err_msgq_reg: + destroy_workqueue(pthread_info->msg_workq); + pthread_info->msg_workq = NULL; +err: + return rc; } int mmrm_vm_msgq_deinit(struct mmrm_vm_driver_data *mmrm_vm) @@ -147,5 +174,9 @@ int mmrm_vm_msgq_deinit(struct mmrm_vm_driver_data *mmrm_vm) if (pmsg_info->msgq_handle) gh_msgq_unregister(pmsg_info->msgq_handle); + + if (pthread_info->msg_workq) + destroy_workqueue(pthread_info->msg_workq); + return 0; }