void socket_stub_input(int socket, void *buf_p, size_t size)
{
struct socket_t *socket_p;
while (1) {
socket_p = sockets[socket];
if (socket_p != NULL) {
break;
}
thrd_sleep_ms(1);
}
/* Resume any polling thread. */
sys_lock();
if (chan_is_polled_isr(&socket_p->base)) {
thrd_resume_isr(socket_p->base.reader_p, 0);
socket_p->base.reader_p = NULL;
}
chan_write_isr(&qinput, &buf_p, sizeof(buf_p));
chan_write_isr(&qinput, &size, sizeof(size));
sys_unlock();
}
ssize_t event_write_isr(struct event_t *self_p,
const void *buf_p,
size_t size)
{
if (chan_is_polled_isr(&self_p->base)) {
thrd_resume_isr(self_p->base.reader_p, 0);
self_p->base.reader_p = NULL;
}
self_p->mask |= *(uint32_t *)buf_p;
/* Resume waiting thread. */
if (self_p->base.reader_p != NULL) {
thrd_resume_isr(self_p->base.reader_p, 0);
self_p->base.reader_p = NULL;
}
return (size);
}
static void resume_thrd(struct thrd_t *thrd_p, int res)
{
/* Resume the reading thread. */
sys_lock();
thrd_resume_isr(thrd_p, res);
sys_unlock();
#if defined(ARCH_ESP) || defined(ARCH_ESP32)
xSemaphoreGive(thrd_idle_sem);
#endif
}
int queue_stop_isr(struct queue_t *self_p)
{
int res = 0;
/* If the reader is from a poll call, the resume value is
ignored. */
if (self_p->base.reader_p != NULL) {
thrd_resume_isr(self_p->base.reader_p, self_p->size - self_p->left);
self_p->base.reader_p = NULL;
res = 1;
}
if (self_p->base.writer_p != NULL) {
thrd_resume_isr(self_p->base.writer_p, self_p->size - self_p->left);
self_p->base.writer_p = NULL;
res = 1;
}
self_p->state = QUEUE_STATE_STOPPED;
return (res);
}
static void resume_if_polled(struct socket_t *socket_p)
{
int polled;
/* Resume any polling thread. */
sys_lock();
polled = chan_is_polled_isr(&socket_p->base);
if (polled == 1) {
thrd_resume_isr(socket_p->base.reader_p, 0);
socket_p->base.reader_p = NULL;
}
sys_unlock();
#if defined(ARCH_ESP) || defined(ARCH_ESP32)
if (polled == 1) {
xSemaphoreGive(thrd_idle_sem);
}
#endif
}
ssize_t queue_write_isr(struct queue_t *self_p,
const void *buf_p,
size_t size)
{
size_t n, left;
size_t buffer_unused_until_end, buffer_unused;
const char *cbuf_p;
left = size;
cbuf_p = buf_p;
/* Resume any polling thread. */
if (chan_is_polled_isr(&self_p->base)) {
thrd_resume_isr(self_p->base.reader_p, 0);
self_p->base.reader_p = NULL;
}
/* Write is not possible to a stopped queue. */
if (self_p->state == QUEUE_STATE_STOPPED) {
return (-1);
}
/* Copy data to the reader, if one is present. */
if (self_p->base.reader_p != NULL) {
if (left < self_p->left) {
n = left;
} else {
n = self_p->left;
}
memcpy(self_p->buf_p, cbuf_p, n);
self_p->buf_p += n;
self_p->left -= n;
cbuf_p += n;
left -= n;
/* Read buffer full. */
if (self_p->left == 0) {
/* Wake the reader. */
thrd_resume_isr(self_p->base.reader_p, self_p->size);
self_p->base.reader_p = NULL;
}
}
if ((left > 0) && (self_p->buffer.begin_p != NULL)) {
buffer_unused = BUFFER_UNUSED(&self_p->buffer);
if (left < buffer_unused) {
n = left;
} else {
n = buffer_unused;
}
buffer_unused_until_end = BUFFER_UNUSED_UNTIL_END(&self_p->buffer);
if (n <= buffer_unused_until_end) {
/* Data fits before the end of the buffer. */
memcpy(self_p->buffer.write_p, cbuf_p, n);
self_p->buffer.write_p += n;
} else {
/* The data does not fit begore the end of the
* buffer. Write until end and then start from the
* beginning. */
memcpy(self_p->buffer.write_p, cbuf_p, buffer_unused_until_end);
memcpy(self_p->buffer.begin_p,
cbuf_p + buffer_unused_until_end,
(n - buffer_unused_until_end));
self_p->buffer.write_p = self_p->buffer.begin_p;
self_p->buffer.write_p += (n - buffer_unused_until_end);
}
left -= n;
}
return (size - left);
}
ssize_t queue_read(struct queue_t *self_p, void *buf_p, size_t size)
{
ASSERTN(self_p != NULL, EINVAL);
ASSERTN(buf_p != NULL, EINVAL);
ASSERTN(size > 0, EINVAL);
size_t left, n, buffer_used_until_end, buffer_used;
char *cbuf_p;
left = size;
cbuf_p = buf_p;
sys_lock();
/* Copy data from queue buffer. */
if (self_p->buffer.begin_p != NULL) {
buffer_used = get_buffer_used(&self_p->buffer);
/* Number of bytes to read from the buffer. */
if (left < buffer_used) {
n = left;
} else {
n = buffer_used;
}
buffer_used_until_end = BUFFER_USED_UNTIL_END(&self_p->buffer);
if (n <= buffer_used_until_end) {
/* Read one chunk. */
memcpy(cbuf_p, self_p->buffer.read_p, n);
self_p->buffer.read_p += n;
} else {
/* Read two chunks, to end and then from beginning. */
memcpy(cbuf_p, self_p->buffer.read_p, buffer_used_until_end);
memcpy(cbuf_p + buffer_used_until_end,
self_p->buffer.begin_p,
(n - buffer_used_until_end));
self_p->buffer.read_p = self_p->buffer.begin_p;
self_p->buffer.read_p += (n - buffer_used_until_end);
}
cbuf_p += n;
left -= n;
}
/* Copy data from the writer, if one is present. */
if (self_p->base.writer_p != NULL) {
if (left < self_p->left) {
n = left;
} else {
n = self_p->left;
}
memcpy(cbuf_p, self_p->buf_p, n);
self_p->buf_p += n;
self_p->left -= n;
cbuf_p += n;
left -= n;
/* Writer buffer empty. */
if (self_p->left == 0) {
/* Wake the writer. */
thrd_resume_isr(self_p->base.writer_p, self_p->size);
self_p->base.writer_p = NULL;
}
}
/* Suspend this thread if more data should be read. */
if (left > 0) {
/* No more data will be written to a stopped queue. */
if (self_p->state == QUEUE_STATE_STOPPED) {
size = (size - left);
} else {
/* The writer writes the remaining data to the reader buffer. */
self_p->base.reader_p = thrd_self();
self_p->buf_p = cbuf_p;
self_p->size = size;
self_p->left = left;
size = thrd_suspend_isr(NULL);
}
}
sys_unlock();
return (size);
}