driver: soc: xilinx: Add support of multiple callbacks for same event in event management driver

As per the current implementation of only single callback data gets
saved per event, driver is throwing an error if try to register multiple
callback for same event.

Add support of multiple callbacks data for same event. So agent can
register for same event with multiple callbacks. Here event management
driver will store the callbacks as list in Hash table entry for that event.

Here each callback data contain 2 element as callback handler and private
data of agent driver.

Signed-off-by: Abhyuday Godhasara <abhyuday.godhasara@xilinx.com>
Link: https://lore.kernel.org/r/20220427074803.19009-2-abhyuday.godhasara@xilinx.com
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

1 files changed, 117 insertions, 34 deletions

diff --git a/drivers/soc/xilinx/xlnx_event_manager.c b/drivers/soc/xilinx/xlnx_event_manager.c
index b27f8853508e..f89000dc33a3 100644
--- a/drivers/soc/xilinx/xlnx_event_manager.c
+++ b/drivers/soc/xilinx/xlnx_event_manager.c
@@ -42,24 +42,34 @@ static DEFINE_HASHTABLE(reg_driver_map, REGISTERED_DRIVER_MAX_ORDER);
 static int sgi_num = XLNX_EVENT_SGI_NUM;
 
 /**
+ * struct agent_cb - Registered callback function and private data.
+ * @agent_data:		Data passed back to handler function.
+ * @eve_cb:		Function pointer to store the callback function.
+ * @list:		member to create list.
+ */
+struct agent_cb {
+	void *agent_data;
+	event_cb_func_t eve_cb;
+	struct list_head list;
+};
+
+/**
  * struct registered_event_data - Registered Event Data.
  * @key:		key is the combine id(Node-Id | Event-Id) of type u64
  *			where upper u32 for Node-Id and lower u32 for Event-Id,
  *			And this used as key to index into hashmap.
- * @agent_data:		Data passed back to handler function.
  * @cb_type:		Type of Api callback, like PM_NOTIFY_CB, etc.
- * @eve_cb:		Function pointer to store the callback function.
- * @wake:		If this flag set, firmware will wakeup processor if is
+ * @wake:		If this flag set, firmware will wake up processor if is
  *			in sleep or power down state.
+ * @cb_list_head:	Head of call back data list which contain the information
+ *			about registered handler and private data.
  * @hentry:		hlist_node that hooks this entry into hashtable.
  */
 struct registered_event_data {
 	u64 key;
 	enum pm_api_cb_id cb_type;
-	void *agent_data;
-
-	event_cb_func_t eve_cb;
 	bool wake;
+	struct list_head cb_list_head;
 	struct hlist_node hentry;
 };
 
@@ -78,29 +88,60 @@ static int xlnx_add_cb_for_notify_event(const u32 node_id, const u32 event, cons
 					event_cb_func_t cb_fun,	void *data)
 {
 	u64 key = 0;
+	bool present_in_hash = false;
 	struct registered_event_data *eve_data;
+	struct agent_cb *cb_data;
+	struct agent_cb *cb_pos;
+	struct agent_cb *cb_next;
 
 	key = ((u64)node_id << 32U) | (u64)event;
 	/* Check for existing entry in hash table for given key id */
 	hash_for_each_possible(reg_driver_map, eve_data, hentry, key) {
 		if (eve_data->key == key) {
-			pr_err("Found as already registered\n");
-			return -EINVAL;
+			present_in_hash = true;
+			break;
 		}
 	}
 
-	/* Add new entry if not present */
-	eve_data = kmalloc(sizeof(*eve_data), GFP_KERNEL);
-	if (!eve_data)
-		return -ENOMEM;
+	if (!present_in_hash) {
+		/* Add new entry if not present in HASH table */
+		eve_data = kmalloc(sizeof(*eve_data), GFP_KERNEL);
+		if (!eve_data)
+			return -ENOMEM;
+		eve_data->key = key;
+		eve_data->cb_type = PM_NOTIFY_CB;
+		eve_data->wake = wake;
+		INIT_LIST_HEAD(&eve_data->cb_list_head);
+
+		cb_data = kmalloc(sizeof(*cb_data), GFP_KERNEL);
+		if (!cb_data)
+			return -ENOMEM;
+		cb_data->eve_cb = cb_fun;
+		cb_data->agent_data = data;
+
+		/* Add into callback list */
+		list_add(&cb_data->list, &eve_data->cb_list_head);
+
+		/* Add into HASH table */
+		hash_add(reg_driver_map, &eve_data->hentry, key);
+	} else {
+		/* Search for callback function and private data in list */
+		list_for_each_entry_safe(cb_pos, cb_next, &eve_data->cb_list_head, list) {
+			if (cb_pos->eve_cb == cb_fun &&
+			    cb_pos->agent_data == data) {
+				return 0;
+			}
+		}
 
-	eve_data->key = key;
-	eve_data->cb_type = PM_NOTIFY_CB;
-	eve_data->eve_cb = cb_fun;
-	eve_data->wake = wake;
-	eve_data->agent_data = data;
+		/* Add multiple handler and private data in list */
+		cb_data = kmalloc(sizeof(*cb_data), GFP_KERNEL);
+		if (!cb_data)
+			return -ENOMEM;
+		cb_data->eve_cb = cb_fun;
+		cb_data->agent_data = data;
 
-	hash_add(reg_driver_map, &eve_data->hentry, key);
+		list_add(&cb_data->list, &eve_data->cb_list_head);
+	}
 
 	return 0;
 }
@@ -108,6 +149,7 @@ static int xlnx_add_cb_for_notify_event(const u32 node_id, const u32 event, cons
 static int xlnx_add_cb_for_suspend(event_cb_func_t cb_fun, void *data)
 {
 	struct registered_event_data *eve_data;
+	struct agent_cb *cb_data;
 
 	/* Check for existing entry in hash table for given cb_type */
 	hash_for_each_possible(reg_driver_map, eve_data, hentry, PM_INIT_SUSPEND_CB) {
@@ -124,8 +166,16 @@ static int xlnx_add_cb_for_suspend(event_cb_func_t cb_fun, void *data)
 
 	eve_data->key = 0;
 	eve_data->cb_type = PM_INIT_SUSPEND_CB;
-	eve_data->eve_cb = cb_fun;
-	eve_data->agent_data = data;
+	INIT_LIST_HEAD(&eve_data->cb_list_head);
+
+	cb_data = kmalloc(sizeof(*cb_data), GFP_KERNEL);
+	if (!cb_data)
+		return -ENOMEM;
+	cb_data->eve_cb = cb_fun;
+	cb_data->agent_data = data;
+
+	/* Add into callback list */
+	list_add(&cb_data->list, &eve_data->cb_list_head);
 
 	hash_add(reg_driver_map, &eve_data->hentry, PM_INIT_SUSPEND_CB);
 
@@ -136,12 +186,20 @@ static int xlnx_remove_cb_for_suspend(event_cb_func_t cb_fun)
 {
 	bool is_callback_found = false;
 	struct registered_event_data *eve_data;
+	struct agent_cb *cb_pos;
+	struct agent_cb *cb_next;
 
 	/* Check for existing entry in hash table for given cb_type */
 	hash_for_each_possible(reg_driver_map, eve_data, hentry, PM_INIT_SUSPEND_CB) {
-		if (eve_data->cb_type == PM_INIT_SUSPEND_CB &&
-		    eve_data->eve_cb == cb_fun) {
-			is_callback_found = true;
+		if (eve_data->cb_type == PM_INIT_SUSPEND_CB) {
+			/* Delete the list of callback */
+			list_for_each_entry_safe(cb_pos, cb_next, &eve_data->cb_list_head, list) {
+				if (cb_pos->eve_cb == cb_fun) {
+					is_callback_found = true;
+					list_del_init(&cb_pos->list);
+					kfree(cb_pos);
+				}
+			}
 			/* remove an object from a hashtable */
 			hash_del(&eve_data->hentry);
 			kfree(eve_data);
@@ -161,13 +219,21 @@ static int xlnx_remove_cb_for_notify_event(const u32 node_id, const u32 event,
 	bool is_callback_found = false;
 	struct registered_event_data *eve_data;
 	u64 key = ((u64)node_id << 32U) | (u64)event;
+	struct agent_cb *cb_pos;
+	struct agent_cb *cb_next;
 
 	/* Check for existing entry in hash table for given key id */
 	hash_for_each_possible(reg_driver_map, eve_data, hentry, key) {
-		if (eve_data->key == key &&
-		    eve_data->eve_cb == cb_fun) {
-			is_callback_found = true;
-			/* remove an object from a hashtable */
+		if (eve_data->key == key) {
+			/* Delete the list of callback */
+			list_for_each_entry_safe(cb_pos, cb_next, &eve_data->cb_list_head, list) {
+				if (cb_pos->eve_cb == cb_fun) {
+					is_callback_found = true;
+					list_del_init(&cb_pos->list);
+					kfree(cb_pos);
+				}
+			}
+			/* remove an object from a HASH table */
 			hash_del(&eve_data->hentry);
 			kfree(eve_data);
 		}
@@ -338,12 +404,16 @@ static void xlnx_call_suspend_cb_handler(const u32 *payload)
 	bool is_callback_found = false;
 	struct registered_event_data *eve_data;
 	u32 cb_type = payload[0];
+	struct agent_cb *cb_pos;
+	struct agent_cb *cb_next;
 
 	/* Check for existing entry in hash table for given cb_type */
 	hash_for_each_possible(reg_driver_map, eve_data, hentry, cb_type) {
 		if (eve_data->cb_type == cb_type) {
-			eve_data->eve_cb(&payload[0], eve_data->agent_data);
-			is_callback_found = true;
+			list_for_each_entry_safe(cb_pos, cb_next, &eve_data->cb_list_head, list) {
+				cb_pos->eve_cb(&payload[0], cb_pos->agent_data);
+				is_callback_found = true;
+			}
 		}
 	}
 	if (!is_callback_found)
@@ -356,12 +426,16 @@ static void xlnx_call_notify_cb_handler(const u32 *payload)
 	struct registered_event_data *eve_data;
 	u64 key = ((u64)payload[1] << 32U) | (u64)payload[2];
 	int ret;
+	struct agent_cb *cb_pos;
+	struct agent_cb *cb_next;
 
 	/* Check for existing entry in hash table for given key id */
 	hash_for_each_possible(reg_driver_map, eve_data, hentry, key) {
 		if (eve_data->key == key) {
-			eve_data->eve_cb(&payload[0], eve_data->agent_data);
-			is_callback_found = true;
+			list_for_each_entry_safe(cb_pos, cb_next, &eve_data->cb_list_head, list) {
+				cb_pos->eve_cb(&payload[0], cb_pos->agent_data);
+				is_callback_found = true;
+			}
 
 			/* re register with firmware to get future events */
 			ret = zynqmp_pm_register_notifier(payload[1], payload[2],
@@ -369,9 +443,12 @@ static void xlnx_call_notify_cb_handler(const u32 *payload)
 			if (ret) {
 				pr_err("%s() failed for 0x%x and 0x%x: %d\r\n", __func__,
 				       payload[1], payload[2], ret);
-				/* Remove already registered event from hash table */
-				xlnx_remove_cb_for_notify_event(payload[1], payload[2],
-								eve_data->eve_cb);
+				list_for_each_entry_safe(cb_pos, cb_next, &eve_data->cb_list_head,
+							 list) {
+					/* Remove already registered event from hash table */
+					xlnx_remove_cb_for_notify_event(payload[1], payload[2],
+									cb_pos->eve_cb);
+				}
 			}
 		}
 	}
@@ -572,8 +649,14 @@ static int xlnx_event_manager_remove(struct platform_device *pdev)
 	struct registered_event_data *eve_data;
 	struct hlist_node *tmp;
 	int ret;
+	struct agent_cb *cb_pos;
+	struct agent_cb *cb_next;
 
 	hash_for_each_safe(reg_driver_map, i, tmp, eve_data, hentry) {
+		list_for_each_entry_safe(cb_pos, cb_next, &eve_data->cb_list_head, list) {
+			list_del_init(&cb_pos->list);
+			kfree(cb_pos);
+		}
 		hash_del(&eve_data->hentry);
 		kfree(eve_data);
 	}