freebsd compat. layer: taskqueue implementation.

git-svn-id: file:///srv/svn/repos/haiku/haiku/trunk@21071 a95241bf-73f2-0310-859d-f6bbb57e9c96

src/libs/compat/freebsd_network/device.c

@@ -399,20 +399,21 @@ _fbsd_init_driver(driver_t *driver)
 	if (dev == NULL)
 		return B_NO_MEMORY;
 
-	init_compat_layer();
+	status = init_compat_layer();
+	if (status < B_OK)
+		goto err_1;
 
 	status = init_mutexes();
-	if (status < B_OK) {
+	if (status < B_OK)
-		free_device(dev);
+		goto err_2;
-		return status;
+
-	}
+	status = init_taskqueues();
+	if (status < B_OK)
+		goto err_3;
 
 	status = init_mbufs();
-	if (status < B_OK) {
+	if (status < B_OK)
-		uninit_mutexes();
+		goto err_4;
-		free_device(dev);
-		return status;
-	}
 
 	init_bounce_pages();
 
@@ -443,6 +444,15 @@ _fbsd_init_driver(driver_t *driver)
 	gDevNameList[ncards + 1] = NULL;
 
 	return B_OK;
+
+err_4:
+	uninit_taskqueues();
+err_3:
+	uninit_mutexes();
+err_2:
+err_1:
+	free(dev);
+	return status;
 }
 
 
@@ -459,5 +469,6 @@ _fbsd_uninit_driver(driver_t *driver)
 
 	uninit_bounce_pages();
 	uninit_mbufs();
+	uninit_taskqueues();
 	uninit_mutexes();
 }

src/libs/compat/freebsd_network/device.h

@@ -62,6 +62,9 @@ void uninit_mutexes(void);
 
 status_t init_compat_layer(void);
 
+status_t init_taskqueues(void);
+void uninit_taskqueues(void);
+
 /* busdma_machdep.c */
 void init_bounce_pages(void);
 void uninit_bounce_pages(void);

src/libs/compat/freebsd_network/taskqueue.c

@@ -8,71 +8,241 @@
 
 #include "device.h"
 
+#include <stdio.h>
+#include <util/list.h>
+
 #include <compat/sys/taskqueue.h>
 
 struct task {
-	int priority;
+	int ta_priority;
-	task_handler_t handler;
+	task_handler_t ta_handler;
-	void *argument;
+	void *ta_argument;
+	int ta_pending;
+
+	struct list_link ta_link;
 };
 
 struct taskqueue {
+	char tq_name[64];
+	mutex tq_mutex;
+	struct list tq_list;
+	taskqueue_enqueue_fn tq_enqueue;
+	void *tq_arg;
+	int tq_fast;
+	int32 tq_spinlock;
+	sem_id tq_sem;
+	thread_id *tq_threads;
+	int tq_threadcount;
 };
 
-struct taskqueue *taskqueue_fast;
+
+struct taskqueue *taskqueue_fast = NULL;
+
+
+static struct taskqueue *
+_taskqueue_create(const char *name, int mflags, int fast,
+	taskqueue_enqueue_fn enqueue, void *context)
+{
+	struct taskqueue *tq = malloc(sizeof(struct taskqueue));
+	if (tq == NULL)
+		return NULL;
+
+	tq->tq_fast = fast;
+
+	if (fast) {
+		tq->tq_spinlock = 0;
+	} else {
+		if (mutex_init(&tq->tq_mutex, name) < B_OK) {
+			free(tq);
+			return NULL;
+		}
+	}
+
+	strlcpy(tq->tq_name, name, sizeof(tq->tq_name));
+	list_init_etc(&tq->tq_list, offsetof(struct task, ta_link));
+	tq->tq_enqueue = enqueue;
+	tq->tq_arg = context;
+
+	tq->tq_sem = -1;
+	tq->tq_threads = NULL;
+	tq->tq_threadcount = 0;
+
+	return tq;
+}
+
+
+static void
+tq_lock(struct taskqueue *tq, cpu_status *status)
+{
+	if (tq->tq_fast) {
+		*status = disable_interrupts();
+		acquire_spinlock(&tq->tq_spinlock);
+	} else {
+		mutex_lock(&tq->tq_mutex);
+	}
+}
+
+
+static void
+tq_unlock(struct taskqueue *tq, cpu_status status)
+{
+	if (tq->tq_fast) {
+		release_spinlock(&tq->tq_spinlock);
+		restore_interrupts(status);
+	} else {
+		mutex_unlock(&tq->tq_mutex);
+	}
+}
+
 
 struct taskqueue *
 taskqueue_create(const char *name, int mflags, taskqueue_enqueue_fn enqueue,
 	void *context, void **unused)
 {
-	UNIMPLEMENTED();
+	return _taskqueue_create(name, mflags, 0, enqueue, context);
-	return NULL;
+}
+
+
+static int32
+tq_handle_thread(void *data)
+{
+	struct taskqueue *tq = data;
+	cpu_status cpu_state;
+	struct task *t;
+	int pending;
+
+	while (1) {
+		status_t status = acquire_sem(tq->tq_sem);
+		if (status < B_OK)
+			break;
+
+		tq_lock(tq, &cpu_state);
+		t = list_remove_head_item(&tq->tq_list);
+		pending = t->ta_pending;
+		t->ta_pending = 0;
+		tq_unlock(tq, cpu_state);
+
+		t->ta_handler(t->ta_argument, pending);
+	}
+
+	return 0;
 }
 
 
 int
-taskqueue_start_threads(struct taskqueue **tq, int count, int pri,
+taskqueue_start_threads(struct taskqueue **tqp, int count, int pri,
-	const char *name, ...)
+	const char *format, ...)
 {
-	UNIMPLEMENTED();
+	struct taskqueue *tq = (*tqp);
-	return -1;
+	char name[64];
+	va_list vl;
+	int i, j;
+
+	if (count == 0)
+		return -1;
+
+	if (tq->tq_threads != NULL)
+		return -1;
+
+	va_start(vl, format);
+	vsnprintf(name, sizeof(name), format, vl);
+	va_end(vl);
+
+	tq->tq_threads = malloc(sizeof(thread_id) * count);
+	if (tq->tq_threads == NULL)
+		return B_NO_MEMORY;
+
+	tq->tq_sem = create_sem(0, tq->tq_name);
+	if (tq->tq_sem < B_OK) {
+		free(tq->tq_threads);
+		tq->tq_threads = NULL;
+		return tq->tq_sem;
+	}
+
+	for (i = 0; i < count; i++) {
+		tq->tq_threads[i] = spawn_kernel_thread(tq_handle_thread, tq->tq_name,
+			B_REAL_TIME_DISPLAY_PRIORITY - 20, tq);
+		if (tq->tq_threads[i] < B_OK) {
+			status_t status = tq->tq_threads[i];
+			for (j = 0; j < i; j++)
+				kill_thread(tq->tq_threads[j]);
+			free(tq->tq_threads);
+			tq->tq_threads = NULL;
+			delete_sem(tq->tq_sem);
+			return status;
+		}
+	}
+
+	for (i = 0; i < count; i++)
+		resume_thread(tq->tq_threads[i]);
+
+	return 0;
 }
 
 
 void
 taskqueue_free(struct taskqueue *tq)
 {
-	UNIMPLEMENTED();
+	/* lock and  drain list? */
+	if (!tq->tq_fast)
+		mutex_destroy(&tq->tq_mutex);
+	if (tq->tq_sem != -1) {
+		int i;
+
+		delete_sem(tq->tq_sem);
+
+		for (i = 0; i < tq->tq_threadcount; i++) {
+			status_t status;
+			wait_for_thread(tq->tq_threads[i], &status);
+		}
+	}
+
+	free(tq);
 }
 
 
 void
 taskqueue_drain(struct taskqueue *tq, struct task *task)
 {
-	UNIMPLEMENTED();
+	cpu_status status;
+
+	tq_lock(tq, &status);
+	if (task->ta_pending != 0)
+		panic("unimplemented, taskqueue drain");
+	tq_unlock(tq, status);
 }
 
 
 int
 taskqueue_enqueue(struct taskqueue *tq, struct task *task)
 {
-	UNIMPLEMENTED();
+	cpu_status status;
-	return -1;
+	tq_lock(tq, &status);
+	/* we don't really support priorities */
+	if (task->ta_pending) {
+		task->ta_pending++;
+	} else {
+		list_add_item(&tq->tq_list, task);
+		task->ta_pending = 1;
+		tq->tq_enqueue(tq->tq_arg);
+	}
+	tq_unlock(tq, status);
+	return 0;
 }
 
 
 void
 taskqueue_thread_enqueue(void *context)
 {
-	UNIMPLEMENTED();
+	struct taskqueue **tqp = context;
+	release_sem_etc((*tqp)->tq_sem, 1, B_DO_NOT_RESCHEDULE);
 }
 
 
 int
-taskqueue_enqueue_fast(struct taskqueue *queue, struct task *task)
+taskqueue_enqueue_fast(struct taskqueue *tq, struct task *task)
 {
-	UNIMPLEMENTED();
+	return taskqueue_enqueue(tq, task);
-	return -1;
 }
 
 
@@ -80,16 +250,39 @@ struct taskqueue *
 taskqueue_create_fast(const char *name, int mflags,
 	taskqueue_enqueue_fn enqueue, void *context)
 {
-	UNIMPLEMENTED();
+	return _taskqueue_create(name, mflags, 1, enqueue, context);
-	return NULL;
 }
 
 
 void
 task_init(struct task *t, int prio, task_handler_t handler, void *context)
 {
-	t->priority = prio;
+	t->ta_priority = prio;
-	t->handler = handler;
+	t->ta_handler = handler;
-	t->argument = context;
+	t->ta_argument = context;
+	t->ta_pending = 0;
 }
 
+
+status_t
+init_taskqueues()
+{
+	taskqueue_fast = taskqueue_create_fast("fast taskq", 0,
+		taskqueue_thread_enqueue, NULL);
+	if (taskqueue_fast == NULL)
+		return B_NO_MEMORY;
+
+	if (taskqueue_start_threads(&taskqueue_fast, 1, 0, "fast taskq") < 0) {
+		taskqueue_free(taskqueue_fast);
+		return B_ERROR;
+	}
+
+	return B_OK;
+}
+
+
+void
+uninit_taskqueues()
+{
+	taskqueue_free(taskqueue_fast);
+}