diff --git a/ds/ringbuffer.c b/ds/ringbuffer.c new file mode 100644 index 0000000..ab6d47a --- /dev/null +++ b/ds/ringbuffer.c @@ -0,0 +1,173 @@ +#include +#include + +size_t ringbuffer_unread(struct ringbuffer *ring_buffer) +{ + if (ring_buffer->r_read_ptr == ring_buffer->r_write_ptr) { + return 0; + } + + if (ring_buffer->r_read_ptr > ring_buffer->r_write_ptr) { + return (ring_buffer->r_size - ring_buffer->r_read_ptr) + + ring_buffer->r_write_ptr; + } else { + return (ring_buffer->r_write_ptr - ring_buffer->r_read_ptr); + } +} + +size_t ringbuffer_avail(struct ringbuffer *ring_buffer) +{ + if (ring_buffer->r_read_ptr == ring_buffer->r_write_ptr) { + return ring_buffer->r_size - 1; + } + + if (ring_buffer->r_read_ptr > ring_buffer->r_write_ptr) { + return ring_buffer->r_read_ptr - ring_buffer->r_write_ptr - 1; + } else { + return (ring_buffer->r_size - ring_buffer->r_write_ptr) + + ring_buffer->r_read_ptr - 1; + } +} + +static inline void increment_read(struct ringbuffer *ring_buffer) +{ + ring_buffer->r_read_ptr++; + if (ring_buffer->r_read_ptr == ring_buffer->r_size) { + ring_buffer->r_read_ptr = 0; + } +} + +static inline void increment_write(struct ringbuffer *ring_buffer) +{ + ring_buffer->r_write_ptr++; + if (ring_buffer->r_write_ptr == ring_buffer->r_size) { + ring_buffer->r_write_ptr = 0; + } +} + +size_t ringbuffer_read(struct ringbuffer *ring_buffer, size_t size, void *p, enum ringbuffer_flags flags) +{ + if (!ring_buffer) { + return 0; + } + + unsigned char *buffer = p; + unsigned long lock_flags; + size_t collected = 0; + + while (collected < size) { + spin_lock_irqsave(&ring_buffer->r_lock, &lock_flags); + while (ringbuffer_unread(ring_buffer) > 0 && collected < size) { + buffer[collected] = ring_buffer->r_buffer[ring_buffer->r_read_ptr]; + increment_read(ring_buffer); + collected++; + } + + wakeup_queue(&ring_buffer->r_wait_writers); + + if (flags & RB_NOBLOCK) { + spin_unlock_irqrestore(&ring_buffer->r_lock, lock_flags); + break; + } + + struct wait_item waiter; + wait_item_init(&waiter, current_thread()); + thread_wait_begin(&waiter, &ring_buffer->r_wait_readers); + spin_unlock_irqrestore(&ring_buffer->r_lock, lock_flags); + + if (collected < size) { + schedule(SCHED_NORMAL); + } + + thread_wait_end(&waiter, &ring_buffer->r_wait_readers); + } + + wakeup_queue(&ring_buffer->r_wait_writers); + return collected; +} + +size_t ringbuffer_write(struct ringbuffer *ring_buffer, size_t size, const void *p, enum ringbuffer_flags flags) +{ + if (!ring_buffer || !size) { + return 0; + } + + const unsigned char *buffer = p; + unsigned long lock_flags; + size_t written = 0; + + while (written < size) { + spin_lock_irqsave(&ring_buffer->r_lock, &lock_flags); + + while (ringbuffer_avail(ring_buffer) > 0 && written < size) { + ring_buffer->r_buffer[ring_buffer->r_write_ptr] = buffer[written]; + increment_write(ring_buffer); + written++; + } + + wakeup_queue(&ring_buffer->r_wait_readers); + + if (flags & RB_NOBLOCK) { + spin_unlock_irqrestore(&ring_buffer->r_lock, lock_flags); + break; + } + + struct wait_item waiter; + wait_item_init(&waiter, current_thread()); + thread_wait_begin(&waiter, &ring_buffer->r_wait_writers); + spin_unlock_irqrestore(&ring_buffer->r_lock, lock_flags); + + if (written < size) { + schedule(SCHED_NORMAL); + } + + thread_wait_end(&waiter, &ring_buffer->r_wait_writers); + } + + wakeup_queue(&ring_buffer->r_wait_readers); + return written; +} + +struct ringbuffer *ringbuffer_create(size_t size) +{ + struct ringbuffer *out = kzalloc(sizeof(struct ringbuffer), VM_NORMAL); + if (!out) { + return NULL; + } + + if (ringbuffer_init(out, size) != KERN_OK) { + kfree(out); + return NULL; + } + + return out; +} + +void ringbuffer_destroy(struct ringbuffer *ring_buffer) +{ + ringbuffer_deinit(ring_buffer); + kfree(ring_buffer); +} + +kern_status_t ringbuffer_init(struct ringbuffer *buf, size_t size) +{ + buf->r_buffer = kmalloc(size, VM_NORMAL); + if (!buf->r_buffer) { + return KERN_NO_MEMORY; + } + + buf->r_write_ptr = 0; + buf->r_read_ptr = 0; + buf->r_size = size; + buf->r_lock = SPIN_LOCK_INIT; + + return KERN_OK; +} + +kern_status_t ringbuffer_deinit(struct ringbuffer *buf) +{ + kfree(buf->r_buffer); + buf->r_buffer = NULL; + + return KERN_OK; +} diff --git a/include/socks/ringbuffer.h b/include/socks/ringbuffer.h new file mode 100644 index 0000000..ab6af1d --- /dev/null +++ b/include/socks/ringbuffer.h @@ -0,0 +1,35 @@ +#ifndef SOCKS_RINGBUFFER_H_ +#define SOCKS_RINGBUFFER_H_ + +#include +#include + +struct ringbuffer { + unsigned char *r_buffer; + size_t r_write_ptr; + size_t r_read_ptr; + size_t r_size; + spin_lock_t r_lock; + struct waitqueue r_wait_readers; + struct waitqueue r_wait_writers; +}; + +enum ringbuffer_flags { + RB_NORMAL = 0x00u, + RB_NOBLOCK = 0x01u, +}; + +extern struct ringbuffer *ringbuffer_create(size_t size); +extern void ringbuffer_destroy(struct ringbuffer *buf); + +extern kern_status_t ringbuffer_init(struct ringbuffer *buf, size_t size); +extern kern_status_t ringbuffer_deinit(struct ringbuffer *buf); + +extern size_t ringbuffer_unread(struct ringbuffer *buf); +extern size_t ringbuffer_avail(struct ringbuffer *buf); +extern size_t ringbuffer_read(struct ringbuffer *buf, size_t size, void *buffer, enum ringbuffer_flags flags); +extern size_t ringbuffer_write(struct ringbuffer *buf, size_t size, const void *buffer, enum ringbuffer_flags flags); + +extern int ringbuffer_write_would_block(struct ringbuffer *buf); + +#endif diff --git a/include/socks/status.h b/include/socks/status.h index 45fb710..27a03e1 100644 --- a/include/socks/status.h +++ b/include/socks/status.h @@ -10,6 +10,7 @@ typedef unsigned int kern_status_t; #define KERN_UNSUPPORTED (4) #define KERN_NO_MEMORY (5) #define KERN_NO_ENTRY (6) +#define KERN_WOULD_BLOCK (7) extern const char *kern_status_string(kern_status_t status);