mcp_forth/runtime_threadutil.c at master · liamHowatt/mcp_forth · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
#include "mcp_forth.h"

#ifndef M4_NO_THREAD

#include <pthread.h>
#include <stdalign.h>
_Static_assert(alignof(pthread_mutex_t) <= 4);
_Static_assert(alignof(pthread_cond_t) <= 4);

typedef struct {
    void * buf;
    unsigned elsize;
    unsigned nels;
    unsigned next_out;
    unsigned len;
    unsigned unfinished_tasks;
    pthread_mutex_t mutex;
    pthread_cond_t not_full_cond;
    pthread_cond_t not_empty_cond;
    pthread_cond_t all_tasks_done_cond;
} queue_t;

static void queue_init(queue_t * q, void * buf, unsigned elsize, unsigned nels)
{
    int res;

    assert(nels);
    assert(elsize);

    q->buf = buf;
    q->elsize = elsize;
    q->nels = nels;
    q->next_out = 0;
    q->len = 0;
    q->unfinished_tasks = 0;
    res = pthread_mutex_init(&q->mutex, NULL);
    assert(res == 0);
    res = pthread_cond_init(&q->not_full_cond, NULL);
    assert(res == 0);
    res = pthread_cond_init(&q->not_empty_cond, NULL);
    assert(res == 0);
    res = pthread_cond_init(&q->all_tasks_done_cond, NULL);
    assert(res == 0);
}

static void queue_put(queue_t * q, const void * el)
{
    int res;

    res = pthread_mutex_lock(&q->mutex);
    assert(res == 0);

    while(q->len == q->nels) {
        res = pthread_cond_wait(&q->not_full_cond, &q->mutex);
        assert(res == 0);
    }

    unsigned next_in = q->next_out + q->len;
    if(next_in >= q->nels) next_in -= q->nels;

    void * next_in_ptr = q->buf + next_in * q->elsize;

    memcpy(next_in_ptr, el, q->elsize);

    if(q->len == 0) {
        res = pthread_cond_signal(&q->not_empty_cond);
        assert(res == 0);
    }

    q->len += 1;

    q->unfinished_tasks += 1;

    res = pthread_mutex_unlock(&q->mutex);
    assert(res == 0);
}

static void queue_get(queue_t * q, void * el)
{
    int res;

    res = pthread_mutex_lock(&q->mutex);
    assert(res == 0);

    while(q->len == 0) {
        res = pthread_cond_wait(&q->not_empty_cond, &q->mutex);
        assert(res == 0);
    }

    void * next_out_ptr = q->buf + q->next_out * q->elsize;

    memcpy(el, next_out_ptr, q->elsize);

    if(q->len == q->nels) {
        res = pthread_cond_signal(&q->not_full_cond);
        assert(res == 0);
    }

    q->len -= 1;

    q->next_out += 1;
    if(q->next_out >= q->nels) q->next_out = 0;

    res = pthread_mutex_unlock(&q->mutex);
    assert(res == 0);
}

static void queue_task_done(queue_t * q)
{
    int res;

    res = pthread_mutex_lock(&q->mutex);
    assert(res == 0);

    assert(q->unfinished_tasks);

    q->unfinished_tasks -= 1;

    if(q->unfinished_tasks == 0) {
        res = pthread_cond_signal(&q->all_tasks_done_cond);
        assert(res == 0);
    }

    res = pthread_mutex_unlock(&q->mutex);
    assert(res == 0);
}

static void queue_join(queue_t * q)
{
    int res;

    res = pthread_mutex_lock(&q->mutex);
    assert(res == 0);

    while(q->unfinished_tasks) {
        res = pthread_cond_wait(&q->all_tasks_done_cond, &q->mutex);
        assert(res == 0);
    }

    res = pthread_mutex_unlock(&q->mutex);
    assert(res == 0);
}

static void queue_destroy(queue_t * q)
{
    int res;

    res = pthread_mutex_destroy(&q->mutex);
    assert(res == 0);
    res = pthread_cond_destroy(&q->not_full_cond);
    assert(res == 0);
    res = pthread_cond_destroy(&q->not_empty_cond);
    assert(res == 0);
    res = pthread_cond_destroy(&q->all_tasks_done_cond);
    assert(res == 0);
}

const m4_runtime_cb_array_t m4_runtime_lib_threadutil[] = {
    {"queue-memsz", {m4_lit, (void *) sizeof(queue_t)}},
    {"queue-init", {m4_f04, queue_init}},
    {"queue-put", {m4_f02, queue_put}},
    {"queue-get", {m4_f02, queue_get}},
    {"queue-task-done", {m4_f01, queue_task_done}},
    {"queue-join", {m4_f01, queue_join}},
    {"queue-destroy", {m4_f01, queue_destroy}},
    {NULL},
};

#endif