Merge "virt: haven: rm_core: Clean up sequence idr earlier"

#define HH_RM_MAX_MSG_SIZE_BYTES \

	(HH_MSGQ_MAX_MSG_SIZE_BYTES - sizeof(struct hh_rm_rpc_hdr))

/**

 * struct hh_rm_connection - Represents a complete message from resource manager

 * @payload: Combined payload of all the fragments without any RPC headers

 * @size: Size of the payload.

 * @msg_id: Message ID from the header.

 * @ret: Linux return code, set in case there was an error processing the connection.

 * @type: HH_RM_RPC_TYPE_RPLY or HH_RM_RPC_TYPE_NOTIF.

 * @num_fragments: total number of fragments expected to be received for this connection.

 * @fragments_received: fragments received so far.

 * @rm_error: For request/reply sequences with standard replies.

 * @seq: Sequence ID for the main message.

 */

struct hh_rm_connection {

	void *payload;

	size_t size;

	u32 msg_id;

	u16 seq;

	void *recv_buff;

	u32 reply_err_code;

	size_t recv_buff_size;

	struct completion seq_done;

	int ret;

	u8 type;

	u8 num_fragments;

	u8 fragments_received;

	void *current_recv_buff;

	 /* only for req/reply sequence */

	u32 rm_error;

	u16 seq;

	struct completion seq_done;

};

static struct task_struct *hh_rm_drv_recv_task;

	struct hh_rm_rpc_hdr *hdr = recv_buff;

	size_t max_buf_size;

	connection->num_fragments = hdr->fragments;

	connection->fragments_received = 0;

	connection->type = hdr->type;

	/* Some of the 'reply' types doesn't contain any payload */

	if (!payload_size)

		return 0;

	/* If the data is split into multiple fragments, allocate a large

	 * enough buffer to hold the payloads for all the fragments.

	 */

	connection->recv_buff = connection->current_recv_buff =

				kzalloc(max_buf_size, GFP_KERNEL);

	if (!connection->recv_buff)

	connection->payload = kzalloc(max_buf_size, GFP_KERNEL);

	if (!connection->payload)

		return -ENOMEM;

	memcpy(connection->recv_buff, recv_buff + hdr_size, payload_size);

	connection->current_recv_buff += payload_size;

	connection->recv_buff_size = payload_size;

	connection->num_fragments = hdr->fragments;

	memcpy(connection->payload, recv_buff + hdr_size, payload_size);

	connection->size = payload_size;

	return 0;

}

}

EXPORT_SYMBOL(hh_rm_unregister_notifier);

static struct hh_rm_connection *

hh_rm_wait_for_notif_fragments(void *recv_buff, size_t recv_buff_size)

static int hh_rm_process_notif(void *msg, size_t msg_size)

{

	struct hh_rm_rpc_hdr *hdr = recv_buff;

	struct hh_rm_rpc_hdr *hdr = msg;

	struct hh_rm_connection *connection;

	u32 notification;

	size_t payload_size;

	void *payload;

	int ret = 0;

	connection = hh_rm_alloc_connection(hdr->msg_id);

	if (IS_ERR_OR_NULL(connection))

		return connection;

	payload_size = recv_buff_size - sizeof(*hdr);

	curr_connection = connection;

	ret = hh_rm_init_connection_buff(connection, recv_buff,

					sizeof(*hdr), payload_size);

	if (ret < 0)

		goto out;

	if (wait_for_completion_interruptible(&connection->seq_done)) {

		ret = -ERESTARTSYS;

		goto out;

	if (curr_connection) {

		pr_err("Received new notification from RM-VM before completing last connection\n");

		return -EINVAL;

	}

	return connection;

	connection = hh_rm_alloc_connection(hdr->msg_id);

	if (!connection)

		return -EINVAL;

out:

	if (hh_rm_init_connection_buff(connection, msg, sizeof(*hdr), msg_size - sizeof(*hdr))) {

		kfree(connection);

	return ERR_PTR(ret);

}

static int hh_rm_process_notif(void *recv_buff, size_t recv_buff_size)

{

	struct hh_rm_connection *connection = NULL;

	struct hh_rm_rpc_hdr *hdr = recv_buff;

	u32 notification = hdr->msg_id;

	void *payload = NULL;

	int ret = 0;

	pr_debug("Notification received from RM-VM: %x\n", notification);

	if (curr_connection) {

		pr_err("Received new notification from RM-VM before completing last connection\n");

		return -EINVAL;

	}

	if (recv_buff_size > sizeof(*hdr))

		payload = recv_buff + sizeof(*hdr);

	/* If the notification payload is split-up into

	 * fragments, wait until all them arrive.

	 */

	if (hdr->fragments) {

		connection = hh_rm_wait_for_notif_fragments(recv_buff,

								recv_buff_size);

		if (IS_ERR_OR_NULL(connection))

	if (hdr->fragments)

		return PTR_ERR(connection);

		/* In the case of fragments, the complete payload

		 * is contained under connection->recv_buff

		 */

		payload = connection->recv_buff;

		recv_buff_size = connection->recv_buff_size;

	}

	payload = connection->payload;

	payload_size = connection->size;

	notification = connection->msg_id;

	pr_debug("Notification received from RM-VM: %x\n", notification);

	switch (notification) {

	case HH_RM_NOTIF_VM_STATUS:

		if (recv_buff_size != sizeof(*hdr) +

		if (payload_size !=

			sizeof(struct hh_rm_notif_vm_status_payload)) {

			pr_err("%s: Invalid size for VM_STATUS notif: %u\n",

				__func__, recv_buff_size - sizeof(*hdr));

				__func__, payload_size);

			ret = -EINVAL;

			goto err;

		}

		break;

	case HH_RM_NOTIF_VM_IRQ_LENT:

		if (recv_buff_size != sizeof(*hdr) +

		if (payload_size !=

			sizeof(struct hh_rm_notif_vm_irq_lent_payload)) {

			pr_err("%s: Invalid size for VM_IRQ_LENT notif: %u\n",

				__func__, recv_buff_size - sizeof(*hdr));

				__func__, payload_size);

			ret = -EINVAL;

			goto err;

		}

		break;

	case HH_RM_NOTIF_VM_IRQ_RELEASED:

		if (recv_buff_size != sizeof(*hdr) +

		if (payload_size !=

			sizeof(struct hh_rm_notif_vm_irq_released_payload)) {

			pr_err("%s: Invalid size for VM_IRQ_REL notif: %u\n",

				__func__, recv_buff_size - sizeof(*hdr));

				__func__, payload_size);

			ret = -EINVAL;

			goto err;

		}

		break;

	case HH_RM_NOTIF_VM_IRQ_ACCEPTED:

		if (recv_buff_size != sizeof(*hdr) +

		if (payload_size !=

			sizeof(struct hh_rm_notif_vm_irq_accepted_payload)) {

			pr_err("%s: Invalid size for VM_IRQ_ACCEPTED notif: %u\n",

				__func__, recv_buff_size - sizeof(*hdr));

				__func__, payload_size);

			ret = -EINVAL;

			goto err;

		}

		break;

	case HH_RM_NOTIF_MEM_SHARED:

		if (recv_buff_size < sizeof(*hdr) +

		if (payload_size <

			sizeof(struct hh_rm_notif_mem_shared_payload)) {

			pr_err("%s: Invalid size for MEM_SHARED notif: %u\n",

				__func__, recv_buff_size - sizeof(*hdr));

				__func__, payload_size);

			ret = -EINVAL;

			goto err;

		}

		break;

	case HH_RM_NOTIF_MEM_RELEASED:

		if (recv_buff_size != sizeof(*hdr) +

		if (payload_size !=

			sizeof(struct hh_rm_notif_mem_released_payload)) {

			pr_err("%s: Invalid size for MEM_RELEASED notif: %u\n",

				__func__, recv_buff_size - sizeof(*hdr));

				__func__, payload_size);

			ret = -EINVAL;

			goto err;

		}

		break;

	case HH_RM_NOTIF_MEM_ACCEPTED:

		if (recv_buff_size != sizeof(*hdr) +

		if (payload_size !=

			sizeof(struct hh_rm_notif_mem_accepted_payload)) {

			pr_err("%s: Invalid size for MEM_ACCEPTED notif: %u\n",

				__func__, recv_buff_size - sizeof(*hdr));

				__func__, payload_size);

			ret = -EINVAL;

			goto err;

		}

		break;

	case HH_RM_NOTIF_VM_CONSOLE_CHARS:

		if (recv_buff_size < sizeof(*hdr) +

		if (payload_size >=

			sizeof(struct hh_rm_notif_vm_console_chars)) {

			struct hh_rm_notif_vm_console_chars *console_chars;

			u16 num_bytes;

			console_chars = recv_buff + sizeof(*hdr);

			console_chars = payload;

			num_bytes = console_chars->num_bytes;

			if (sizeof(*hdr) + sizeof(*console_chars) + num_bytes !=

				recv_buff_size) {

			if (sizeof(*console_chars) + num_bytes !=

				payload_size) {

				pr_err("%s: Invalid size for VM_CONSOLE_CHARS notify %u\n",

				       __func__, recv_buff_size - sizeof(*hdr));

				       __func__, payload_size);

				ret = -EINVAL;

				goto err;

			}

		} else {

			pr_err("%s: Invalid size for VM_CONSOLE_CHARS notify %u\n",

				__func__, payload_size);

			goto err;

		}

		break;

	default:

err:

	if (connection) {

		kfree(connection->recv_buff);

		kfree(payload);

		kfree(connection);

	}

	if (ret < 0)

		return ret;

	connection->reply_err_code = reply_hdr->err_code;

	connection->rm_error = reply_hdr->err_code;

	/*

	 * If the data is composed of a single message, wakeup the

	payload_size = recv_buff_size - sizeof(*hdr);

	/* Keep appending the data to the previous fragment's end */

	memcpy(connection->current_recv_buff,

	memcpy(connection->payload + connection->size,

		recv_buff + sizeof(*hdr), payload_size);

	connection->current_recv_buff += payload_size;

	connection->recv_buff_size += payload_size;

	connection->size += payload_size;

	connection->fragments_received++;

	if (connection->fragments_received == connection->num_fragments) {

	struct work_struct recv_work;

};

static void hh_rm_abort_connection(struct hh_rm_connection *connection)

{

	switch (connection->type) {

	case HH_RM_RPC_TYPE_RPLY:

		connection->ret = -EIO;

		complete(&connection->seq_done);

		break;

	case HH_RM_RPC_TYPE_NOTIF:

		fallthrough;

	default:

		kfree(connection->payload);

		kfree(connection);

	}

}

static void hh_rm_process_recv_work(struct work_struct *work)

{

	struct hh_rm_msgq_data *msgq_data;

	switch (hdr->type) {

	case HH_RM_RPC_TYPE_NOTIF:

		if (curr_connection) {

			/* Not possible per protocol. Do something better than BUG_ON */

			pr_warn("Received start of new notification without finishing existing message series.\n");

			hh_rm_abort_connection(curr_connection);

		}

		hh_rm_process_notif(recv_buff, msgq_data->recv_buff_size);

		break;

	case HH_RM_RPC_TYPE_RPLY:

		if (curr_connection) {

			/* Not possible per protocol. Do something better than BUG_ON */

			pr_warn("Received start of new reply without finishing existing message series.\n");

			hh_rm_abort_connection(curr_connection);

		}

		hh_rm_process_rply(recv_buff, msgq_data->recv_buff_size);

		break;

	case HH_RM_RPC_TYPE_CONT:

		if (!curr_connection) {

			/* Not possible per protocol. Do something better than BUG_ON */

			pr_warn("Received a continuation message without receiving initial message\n");

			break;

		}

		hh_rm_process_cont(recv_buff, msgq_data->recv_buff_size);

		break;

	default:

	unsigned long tx_flags;

	u32 num_fragments = 0;

	size_t payload_size;

	void *send_buff;

	int i, ret;

	void *msg;

	int i, ret = 0;

	num_fragments = (req_buff_size + HH_RM_MAX_MSG_SIZE_BYTES - 1) /

			HH_RM_MAX_MSG_SIZE_BYTES;

		return -E2BIG;

	}

	msg = kzalloc(HH_RM_MAX_MSG_SIZE_BYTES, GFP_KERNEL);

	if (!msg)

		return -ENOMEM;

	if (mutex_lock_interruptible(&hh_rm_send_lock)) {

		return -ERESTARTSYS;

		ret = -ERESTARTSYS;

		goto free_msg;

	}

	/* Consider also the 'request' packet for the loop count */

	for (i = 0; i <= num_fragments; i++) {

		if (buff_size_remaining > HH_RM_MAX_MSG_SIZE_BYTES) {

			payload_size = HH_RM_MAX_MSG_SIZE_BYTES;

			payload_size = buff_size_remaining;

		}

		send_buff = kzalloc(sizeof(*hdr) + payload_size, GFP_KERNEL);

		if (!send_buff) {

			mutex_unlock(&hh_rm_send_lock);

			return -ENOMEM;

		}

		memset(msg, 0, HH_RM_MAX_MSG_SIZE_BYTES);

		/* Fill header */

		hdr = msg;

		hdr = send_buff;

		hdr->version = HH_RM_RPC_HDR_VERSION_ONE;

		hdr->hdr_words = HH_RM_RPC_HDR_WORDS;

		hdr->type = i == 0 ? HH_RM_RPC_TYPE_REQ : HH_RM_RPC_TYPE_CONT;

		hdr->seq = connection->seq;

		hdr->msg_id = message_id;

		memcpy(send_buff + sizeof(*hdr), req_buff_curr, payload_size);

		/* Copy payload */

		memcpy(msg + sizeof(*hdr), req_buff_curr, payload_size);

		req_buff_curr += payload_size;

		/* Force the last fragment (or the request type)

		 * to be sent immediately to the receiver

		/* Force the last fragment to be sent immediately to

		 * the receiver

		 */

		tx_flags = (i == num_fragments) ? HH_MSGQ_TX_PUSH : 0;

		    message_id == HH_RM_RPC_MSG_ID_CALL_VM_CONSOLE_FLUSH)

			udelay(800);

		ret = hh_msgq_send(hh_rm_msgq_desc, send_buff,

		ret = hh_msgq_send(hh_rm_msgq_desc, msg,

					sizeof(*hdr) + payload_size, tx_flags);

		/*

		 * In the case of a success, the hypervisor would have consumed

		 * the buffer. While in the case of a failure, we are going to

		 * quit anyways. Hence, free the buffer regardless of the

		 * return value.

		 */

		kfree(send_buff);

		if (ret) {

			mutex_unlock(&hh_rm_send_lock);

			return ret;

		}

		if (ret)

			break;

	}

	mutex_unlock(&hh_rm_send_lock);

	return 0;

free_msg:

	kfree(msg);

	return ret;

}

/**

 * @req_buff: Request buffer that contains the payload

 * @req_buff_size: Total size of the payload

 * @resp_buff_size: Size of the response buffer

 * @reply_err_code: Returns Haven standard error code for the response

 * @rm_error: Returns Haven standard error code for the response

 *

 * Make a request to the RM-VM and expect a reply back. For a successful

 * response, the function returns the payload and its size for the response.

 */

void *hh_rm_call(hh_rm_msgid_t message_id,

			void *req_buff, size_t req_buff_size,

			size_t *resp_buff_size, int *reply_err_code)

			size_t *resp_buff_size, int *rm_error)

{

	struct hh_rm_connection *connection;

	int req_ret;

	void *ret;

	if (!message_id || !req_buff || !resp_buff_size || !reply_err_code)

	if (!message_id || !req_buff || !resp_buff_size || !rm_error)

		return ERR_PTR(-EINVAL);

	connection = hh_rm_alloc_connection(message_id);

		goto out;

	}

	*reply_err_code = connection->reply_err_code;

	if (connection->reply_err_code) {

	mutex_lock(&hh_rm_call_idr_lock);

	idr_remove(&hh_rm_call_idr, connection->seq);

	mutex_unlock(&hh_rm_call_idr_lock);

	*rm_error = connection->rm_error;

	if (connection->rm_error) {

		pr_err("%s: Reply for seq:%d failed with RM err: %d\n",

			__func__, connection->seq, connection->reply_err_code);

		ret = ERR_PTR(hh_remap_error(connection->reply_err_code));

		kfree(connection->recv_buff);

			__func__, connection->seq, connection->rm_error);

		ret = ERR_PTR(hh_remap_error(connection->rm_error));

		kfree(connection->payload);

		goto out;

	}

	if (connection->ret) {

		ret = ERR_PTR(connection->ret);

		kfree(connection->payload);

		goto out;

	}

	print_hex_dump_debug("hh_rm_call RX: ", DUMP_PREFIX_OFFSET, 4, 1,

			     connection->recv_buff, connection->recv_buff_size,

			     connection->payload, connection->size,

			     false);

	ret = connection->recv_buff;

	*resp_buff_size = connection->recv_buff_size;

	ret = connection->payload;

	*resp_buff_size = connection->size;

out:

	mutex_lock(&hh_rm_call_idr_lock);

	idr_remove(&hh_rm_call_idr, connection->seq);

	mutex_unlock(&hh_rm_call_idr_lock);

	kfree(connection);

	return ret;

}