Add queues module

Signed-off-by: Ben Goz <ben.goz@amd.com>
Signed-off-by: Oded Gabbay <oded.gabbay@amd.com>

2 files changed, 136 insertions, 3 deletions

diff --git a/src/libhsakmt.h b/src/libhsakmt.h
index 8972ba9..4d337e6 100644
--- a/src/libhsakmt.h
+++ b/src/libhsakmt.h
@@ -67,6 +67,7 @@ uint16_t get_device_id_by_node(HSAuint32 node_id);
 extern int kfd_ioctl(int cmdcode, void* data);
 
 /* Void pointer arithmetic (or remove -Wpointer-arith to allow void pointers arithmetic) */
+#define VOID_PTR_ADD32(ptr,n) (void*)((uint32_t*)(ptr) + n)/*ptr + offset*/
 #define VOID_PTR_ADD(ptr,n) (void*)((uint8_t*)(ptr) + n)/*ptr + offset*/
 #define VOID_PTR_SUB(ptr,n) (void*)((uint8_t*)(ptr) - n)/*ptr - offset*/
 #define VOID_PTRS_SUB(ptr1,ptr2) (uint64_t)((uint8_t*)(ptr1) - (uint8_t*)(ptr2)) /*ptr1 - ptr2*/
diff --git a/src/queues.c b/src/queues.c
index 3c1c4ff..1292b20 100644
--- a/src/queues.c
+++ b/src/queues.c
@@ -27,6 +27,28 @@
 #include "linux/kfd_ioctl.h"
 #include <stdlib.h>
 #include <string.h>
+#include <sys/mman.h>
+#include <math.h>
+#include <stdio.h>
+
+/* 1024 doorbells, 4 bytes each doorbell */
+#define DOORBELLS_PAGE_SIZE	1024 * 4
+
+struct queue
+{
+	uint32_t queue_id;
+	uint32_t wptr;
+	uint32_t rptr;
+};
+
+struct process_doorbells
+{
+	bool need_mmap;
+	void* doorbells;
+	pthread_mutex_t doorbells_mutex;
+};
+
+struct process_doorbells doorbells[] = {[0 ... (NUM_OF_SUPPORTED_GPUS-1)] = {.need_mmap = true, .doorbells = NULL, .doorbells_mutex = PTHREAD_MUTEX_INITIALIZER}};
 
 HSAKMT_STATUS
 HSAKMTAPI
@@ -41,9 +63,82 @@ hsaKmtCreateQueue(
     HsaQueueResource*   QueueResource     //OUT
     )
 {
+	HSAKMT_STATUS result;
+	uint32_t gpu_id;
+	int err;
+	void* ptr;
 	CHECK_KFD_OPEN();
 
-	return HSAKMT_STATUS_NOT_SUPPORTED;
+	result = validate_nodeid(NodeId, &gpu_id);
+	if (result != HSAKMT_STATUS_SUCCESS)
+		return result;
+
+	struct queue *q = malloc(sizeof(struct queue));
+	if (q == NULL)
+	{
+		return HSAKMT_STATUS_NO_MEMORY;
+	}
+
+	memset(q, 0, sizeof(*q));
+
+	struct kfd_ioctl_create_queue_args args;
+	memset(&args, 0, sizeof(args));
+
+	args.gpu_id = gpu_id;
+
+	switch (Type)
+	{
+	case HSA_QUEUE_COMPUTE: args.queue_type = KFD_IOC_QUEUE_TYPE_COMPUTE; break;
+	case HSA_QUEUE_SDMA: free(q); return HSAKMT_STATUS_NOT_IMPLEMENTED;
+	case HSA_QUEUE_COMPUTE_AQL: args.queue_type = KFD_IOC_QUEUE_TYPE_COMPUTE_AQL; break;
+	default: free(q); return HSAKMT_STATUS_INVALID_PARAMETER;
+	}
+
+	if (Type != HSA_QUEUE_COMPUTE_AQL)
+	{
+		QueueResource->QueueRptrValue = (uintptr_t)&q->rptr;
+		QueueResource->QueueWptrValue = (uintptr_t)&q->wptr;
+	}
+
+	args.read_pointer_address = QueueResource->QueueRptrValue;
+	args.write_pointer_address = QueueResource->QueueWptrValue;
+	args.ring_base_address = (uintptr_t)QueueAddress;
+	args.ring_size = QueueSizeInBytes;
+	args.queue_percentage = QueuePercentage;
+	args.queue_priority = Priority;
+
+	err = kfd_ioctl(KFD_IOC_CREATE_QUEUE, &args);
+
+	if (err == -1)
+	{
+		free(q);
+		return HSAKMT_STATUS_ERROR;
+	}
+
+	q->queue_id = args.queue_id;
+
+	pthread_mutex_lock(&doorbells[NodeId].doorbells_mutex);
+
+	if (doorbells[NodeId].need_mmap) {
+		ptr = mmap(0, DOORBELLS_PAGE_SIZE, PROT_READ|PROT_WRITE,
+				MAP_SHARED, kfd_fd, args.doorbell_offset);
+
+		if (ptr == MAP_FAILED) {
+			pthread_mutex_unlock(&doorbells[NodeId].doorbells_mutex);
+			hsaKmtDestroyQueue(q->queue_id);
+			return HSAKMT_STATUS_ERROR;
+		}
+
+		doorbells[NodeId].need_mmap = false;
+		doorbells[NodeId].doorbells = ptr;
+	}
+
+	pthread_mutex_unlock(&doorbells[NodeId].doorbells_mutex);
+
+	QueueResource->QueueId = PORT_VPTR_TO_UINT64(q);
+	QueueResource->Queue_DoorBell = VOID_PTR_ADD32(doorbells[NodeId].doorbells, q->queue_id);
+
+	return HSAKMT_STATUS_SUCCESS;
 }
 
 
@@ -58,9 +153,26 @@ hsaKmtUpdateQueue(
     HsaEvent*           Event           //IN
     )
 {
+	struct kfd_ioctl_update_queue_args arg;
+	struct queue *q = PORT_UINT64_TO_VPTR(QueueId);
+
 	CHECK_KFD_OPEN();
 
-	return HSAKMT_STATUS_NOT_SUPPORTED;
+	if (q == NULL)
+		return (HSAKMT_STATUS_INVALID_PARAMETER);
+	arg.queue_id = (HSAuint32)q->queue_id;
+	arg.ring_base_address = (uintptr_t)QueueAddress;
+	arg.ring_size = QueueSize;
+	arg.queue_percentage = QueuePercentage;
+	arg.queue_priority = Priority;
+
+	int err = kfd_ioctl(KFD_IOC_UPDATE_QUEUE, &arg);
+	if (err == -1)
+	{
+		return HSAKMT_STATUS_ERROR;
+	}
+
+	return HSAKMT_STATUS_SUCCESS;
 }
 
 HSAKMT_STATUS
@@ -71,5 +183,25 @@ hsaKmtDestroyQueue(
 {
 	CHECK_KFD_OPEN();
 
-	return HSAKMT_STATUS_NOT_SUPPORTED;
+	struct queue *q = PORT_UINT64_TO_VPTR(QueueId);
+	struct kfd_ioctl_destroy_queue_args args;
+
+	if (q == NULL)
+			return (HSAKMT_STATUS_INVALID_PARAMETER);
+
+	memset(&args, 0, sizeof(args));
+
+	args.queue_id = q->queue_id;
+
+	int err = kfd_ioctl(KFD_IOC_DESTROY_QUEUE, &args);
+
+	if (err == -1)
+	{
+		return HSAKMT_STATUS_ERROR;
+	}
+	else
+	{
+		free(q);
+		return HSAKMT_STATUS_SUCCESS;
+	}
 }