linux-nv-oot/drivers/platform/tegra/nvadsp/mailbox.c

// SPDX-License-Identifier: GPL-2.0-only
// SPDX-FileCopyrightText: Copyright (c) 2014-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.

#include "dev.h"
#include "hwmailbox.h"
#include <linux/nospec.h>
#include <asm/barrier.h>

#define NVADSP_MAILBOX_START	512

static inline bool is_mboxq_empty(struct nvadsp_mbox_queue *queue)
{
	return (queue->count == 0);
}

static inline bool is_mboxq_full(struct nvadsp_mbox_queue *queue)
{
	return (queue->count == NVADSP_MBOX_QUEUE_SIZE);
}

static void mboxq_init(struct nvadsp_mbox_queue *queue)
{
	queue->head = 0;
	queue->tail = 0;
	queue->count = 0;
	init_completion(&queue->comp);
	spin_lock_init(&queue->lock);
}

static void mboxq_destroy(struct nvadsp_mbox_queue *queue)
{
	if (!is_mboxq_empty(queue))
		pr_info("Mbox queue %p is not empty.\n", queue);

	queue->head = 0;
	queue->tail = 0;
	queue->count = 0;
}

static status_t mboxq_enqueue(struct nvadsp_mbox_queue *queue,
				   uint32_t data)
{
	unsigned long flags;
	int ret = 0;

	if (is_mboxq_full(queue)) {
		ret = -EINVAL;
		goto out;
	}
	spin_lock_irqsave(&queue->lock, flags);
	if (is_mboxq_empty(queue))
		complete_all(&queue->comp);

	queue->array[queue->tail] = data;
	queue->tail = (queue->tail + 1) & NVADSP_MBOX_QUEUE_SIZE_MASK;
	queue->count++;
	spin_unlock_irqrestore(&queue->lock, flags);
 out:
	return ret;
}

status_t nvadsp_mboxq_enqueue(struct nvadsp_mbox_queue *queue,
					    uint32_t data)
{
	return mboxq_enqueue(queue, data);
}

static status_t mboxq_dequeue(struct nvadsp_mbox_queue *queue,
					  uint32_t *data)
{
	unsigned long flags;
	int ret = 0;

	spin_lock_irqsave(&queue->lock, flags);
	if (is_mboxq_empty(queue)) {
		ret = -EBUSY;
		goto comp;
	}

	*data = queue->array[queue->head];
	queue->head = (queue->head + 1) & NVADSP_MBOX_QUEUE_SIZE_MASK;
	queue->count--;

	if (is_mboxq_empty(queue))
		goto comp;
	else
		goto out;
 comp:
	reinit_completion(&queue->comp);
 out:
	spin_unlock_irqrestore(&queue->lock, flags);
	return ret;
}

static void mboxq_dump(struct nvadsp_mbox_queue *queue)
{
	unsigned long flags;
	uint16_t head, count;
	uint32_t data;

	spin_lock_irqsave(&queue->lock, flags);

	count = queue->count;
	pr_info("nvadsp: queue %p count:%d\n", queue, count);

	pr_info("nvadsp: queue data: ");
	head = queue->head;
	while (count) {
		data = queue->array[head];
		head = (head + 1) & NVADSP_MBOX_QUEUE_SIZE_MASK;
		count--;
		pr_info("0x%x ", data);
	}
	pr_info(" dumped\n");

	spin_unlock_irqrestore(&queue->lock, flags);
}

static uint16_t nvadsp_mbox_alloc_mboxid(
			struct nvadsp_drv_data *nvadsp_drv_data)
{
	unsigned long start = NVADSP_MAILBOX_START;
	unsigned int nr = 1;
	unsigned long align = 0;
	uint16_t mid;

	mid = bitmap_find_next_zero_area(nvadsp_drv_data->mbox_ids,
					 NVADSP_MAILBOX_MAX - 1,
					 start, nr, align);

	bitmap_set(nvadsp_drv_data->mbox_ids, mid, 1);
	return mid;
}

static status_t nvadsp_mbox_free_mboxid(
			struct nvadsp_drv_data *nvadsp_drv_data, uint16_t mid)
{
	bitmap_clear(nvadsp_drv_data->mbox_ids, mid, 1);
	return 0;
}

static void _nvadsp_mbox_set_ack_handler(struct nvadsp_handle *nvadsp_handle,
				struct nvadsp_mbox *mbox,
				nvadsp_mbox_handler_t handler)
{
	mbox->ack_handler = handler;
}

static status_t _nvadsp_mbox_open(struct nvadsp_handle *nvadsp_handle,
				struct nvadsp_mbox *mbox,
				uint16_t *mid, const char *name,
				nvadsp_mbox_handler_t handler, void *hdata)
{
	struct nvadsp_drv_data *nvadsp_drv_data =
				(struct nvadsp_drv_data *)nvadsp_handle;
	unsigned long flags;
	int ret = 0;

	if (!nvadsp_drv_data || !nvadsp_drv_data->pdev) {
		ret = -ENOSYS;
		goto err;
	}

	spin_lock_irqsave(&nvadsp_drv_data->mbox_lock, flags);

	if (!mbox) {
		ret = -EINVAL;
		goto out;
	}

	if (*mid == 0) {
		mbox->id = nvadsp_mbox_alloc_mboxid(nvadsp_drv_data);
		if (mbox->id >= NVADSP_MAILBOX_MAX) {
			ret = -ENOMEM;
			mbox->id = 0;
			goto out;
		}
		*mid = mbox->id;
	} else {
		if (*mid >= NVADSP_MAILBOX_MAX) {
			pr_debug("%s: Invalid mailbox %d.\n",
				 __func__, *mid);
			ret = -ERANGE;
			goto out;
		}

		*mid = array_index_nospec(*mid, NVADSP_MAILBOX_MAX);

		if (nvadsp_drv_data->mboxes[*mid]) {
			pr_debug("%s: mailbox %d already opened.\n",
				 __func__, *mid);
			ret = -EADDRINUSE;
			goto out;
		}
		mbox->id = *mid;
	}

	strncpy(mbox->name, name, NVADSP_MBOX_NAME_MAX);
	mboxq_init(&mbox->recv_queue);
	mbox->handler = handler;
	mbox->hdata = hdata;

	nvadsp_drv_data->mboxes[mbox->id] = mbox;
 out:
	spin_unlock_irqrestore(&nvadsp_drv_data->mbox_lock, flags);
 err:
	return ret;
}

static status_t _nvadsp_mbox_send(struct nvadsp_handle *nvadsp_handle,
				struct nvadsp_mbox *mbox, uint32_t data,
				uint32_t flags, bool block, unsigned int timeout)
{
	struct nvadsp_drv_data *nvadsp_drv_data =
				(struct nvadsp_drv_data *)nvadsp_handle;
	struct hwmbox_queue *hwmbox_send_queue;
	int ret = 0;

	if (!nvadsp_drv_data || !nvadsp_drv_data->pdev ||
			!nvadsp_drv_data->hwmbox_send_queue) {
		pr_err("ADSP drv_data is NULL\n");
		ret = -ENOSYS;
		goto out;
	}

	if (!mbox) {
		pr_err("ADSP MBOX is NULL\n");
		ret = -EINVAL;
		goto out;
	}

	hwmbox_send_queue = nvadsp_drv_data->hwmbox_send_queue;

 retry:
	ret = nvadsp_hwmbox_send_data(nvadsp_drv_data, mbox->id, data, flags);
	if (!ret)
		goto out;

	if (ret == -EBUSY) {
		if (block) {
			ret = wait_for_completion_timeout(
				&hwmbox_send_queue->comp,
				msecs_to_jiffies(timeout));
			if (ret) {
				pr_warn("ADSP HWMBOX send retry\n");
				block = false;
				goto retry;
			} else {
				pr_err("ADSP wait for completion timed out\n");
				ret = -ETIME;
				goto out;
			}
		} else {
			pr_debug("Failed to enqueue data 0x%x. ret: %d\n",
				 data, ret);
		}
	} else if (ret) {
		pr_warn("Failed to enqueue data 0x%x. ret: %d\n", data, ret);
		goto out;
	}
 out:
	return ret;
}

static status_t _nvadsp_mbox_recv(struct nvadsp_handle *nvadsp_handle,
				struct nvadsp_mbox *mbox, uint32_t *data,
				bool block, unsigned int timeout)
{
	struct nvadsp_drv_data *nvadsp_drv_data =
				(struct nvadsp_drv_data *)nvadsp_handle;
	int ret = 0;

	if (!nvadsp_drv_data || !nvadsp_drv_data->pdev) {
		ret = -ENOSYS;
		goto out;
	}

	if (!mbox) {
		ret = -EINVAL;
		goto out;
	}

 retry:
	ret = mboxq_dequeue(&mbox->recv_queue, data);
	if (!ret)
		goto out;

	if (ret == -EBUSY) {
		if (block) {
			ret = wait_for_completion_timeout(
					  &mbox->recv_queue.comp,
					  msecs_to_jiffies(timeout));
			if (ret) {
				block = false;
				goto retry;
			} else {
				ret = -ETIME;
				goto out;
			}
		} else {
			pr_debug("Failed to receive data.  ret: %d\n", ret);
		}
	} else if (ret) {
		pr_debug("Failed to receive data. ret: %d\n", ret);
		goto out;
	}
 out:
	return ret;
}

static status_t _nvadsp_mbox_close(struct nvadsp_handle *nvadsp_handle,
				struct nvadsp_mbox *mbox)
{
	struct nvadsp_drv_data *nvadsp_drv_data =
				(struct nvadsp_drv_data *)nvadsp_handle;
	unsigned long flags;
	int ret = 0;

	if (!nvadsp_drv_data || !nvadsp_drv_data->pdev) {
		ret = -ENOSYS;
		goto err;
	}

	spin_lock_irqsave(&nvadsp_drv_data->mbox_lock, flags);
	if (!mbox) {
		ret = -EINVAL;
		goto out;
	}

	if (!is_mboxq_empty(&mbox->recv_queue)) {
		ret = -ENOTEMPTY;
		mboxq_dump(&mbox->recv_queue);
		goto out;
	}

	nvadsp_mbox_free_mboxid(nvadsp_drv_data, mbox->id);
	mboxq_destroy(&mbox->recv_queue);
	nvadsp_drv_data->mboxes[mbox->id] = NULL;
 out:
	spin_unlock_irqrestore(&nvadsp_drv_data->mbox_lock, flags);
 err:
	return ret;
}

status_t nvadsp_mbox_init(struct platform_device *pdev)
{
	struct nvadsp_drv_data *drv = platform_get_drvdata(pdev);
	struct nvadsp_handle *nvadsp_handle = &drv->nvadsp_handle;

	memset(drv->mboxes, 0, sizeof(drv->mboxes));
	memset(drv->mbox_ids, 0, sizeof(drv->mbox_ids));

	spin_lock_init(&drv->mbox_lock);

	nvadsp_handle->mbox_open   = _nvadsp_mbox_open;
	nvadsp_handle->mbox_close  = _nvadsp_mbox_close;
	nvadsp_handle->mbox_send   = _nvadsp_mbox_send;
	nvadsp_handle->mbox_recv   = _nvadsp_mbox_recv;
	nvadsp_handle->mbox_set_ack_handler = _nvadsp_mbox_set_ack_handler;

	return 0;
}