From d4c3066e7c5efa5f395c21db77609516d386cbf8 Mon Sep 17 00:00:00 2001 From: Felix Fietkau Date: Mon, 27 Nov 2023 15:44:33 +0100 Subject: [PATCH] udebug: add udebug library code Copied and adapted from udebug.git Signed-off-by: Felix Fietkau --- CMakeLists.txt | 3 +- udebug-priv.h | 54 ++++ udebug-proto.h | 72 +++++ udebug-remote.c | 382 ++++++++++++++++++++++++ udebug.c | 758 ++++++++++++++++++++++++++++++++++++++++++++++++ udebug.h | 196 +++++++++++++ 6 files changed, 1464 insertions(+), 1 deletion(-) create mode 100644 udebug-priv.h create mode 100644 udebug-proto.h create mode 100644 udebug-remote.c create mode 100644 udebug.c create mode 100644 udebug.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 1a1b7d5..1a6ff58 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,7 +21,7 @@ IF(JSONC_FOUND) INCLUDE_DIRECTORIES(${JSONC_INCLUDE_DIRS}) ENDIF() -SET(SOURCES avl.c avl-cmp.c blob.c blobmsg.c uloop.c usock.c ustream.c ustream-fd.c vlist.c utils.c safe_list.c runqueue.c md5.c kvlist.c ulog.c base64.c) +SET(SOURCES avl.c avl-cmp.c blob.c blobmsg.c uloop.c usock.c ustream.c ustream-fd.c vlist.c utils.c safe_list.c runqueue.c md5.c kvlist.c ulog.c base64.c udebug.c udebug-remote.c) ADD_LIBRARY(ubox SHARED ${SOURCES}) ADD_LIBRARY(ubox-static STATIC ${SOURCES}) @@ -37,6 +37,7 @@ IF(NOT HAVE_GETTIME) ENDIF() FILE(GLOB headers *.h) +LIST(FILTER headers EXCLUDE REGEX "-priv.h$" ) INSTALL(FILES ${headers} DESTINATION include/libubox ) diff --git a/udebug-priv.h b/udebug-priv.h new file mode 100644 index 0000000..9e43ddf --- /dev/null +++ b/udebug-priv.h @@ -0,0 +1,54 @@ +/* + * udebug - debug ring buffer library + * + * Copyright (C) 2023 Felix Fietkau + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + */ +#ifndef __UDEBUG_UTIL_H +#define __UDEBUG_UTIL_H + +#include "blobmsg.h" +#include "udebug.h" +#include "udebug-proto.h" + +#define UDEBUG_TIMEOUT 1000 + +__hidden int udebug_buf_open(struct udebug_buf *buf, int fd, uint32_t ring_size, uint32_t data_size); +__hidden struct udebug_client_msg *__udebug_poll(struct udebug *ctx, int *fd, bool wait); +__hidden void udebug_send_msg(struct udebug *ctx, struct udebug_client_msg *msg, + struct blob_attr *meta, int fd); + +static inline int32_t u32_sub(uint32_t a, uint32_t b) +{ + return a - b; +} + +static inline int32_t u32_max(uint32_t a, uint32_t b) +{ + return u32_sub(a, b) > 0 ? a : b; +} + +static inline void u32_set(void *ptr, uint32_t val) +{ + volatile uint32_t *v = ptr; + *v = val; +} + +static inline uint32_t u32_get(void *ptr) +{ + volatile uint32_t *v = ptr; + return *v; +} + +#endif diff --git a/udebug-proto.h b/udebug-proto.h new file mode 100644 index 0000000..916c5fa --- /dev/null +++ b/udebug-proto.h @@ -0,0 +1,72 @@ +/* + * udebug - debug ring buffer library + * + * Copyright (C) 2023 Felix Fietkau + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + */ +#ifndef __UDEBUG_PROTO_H +#define __UDEBUG_PROTO_H + +#include "udebug.h" + +struct udebug_hdr { + uint32_t ring_size; + uint32_t data_size; + + uint32_t format; + uint32_t sub_format; + + uintptr_t flags[8 / sizeof(uintptr_t)]; + uintptr_t notify; + + uint32_t head_hi; + uint32_t head; + uint32_t data_head; + uint32_t data_used; +}; + +enum udebug_client_msg_type { + CL_MSG_RING_ADD, + CL_MSG_RING_REMOVE, + CL_MSG_RING_NOTIFY, + CL_MSG_GET_HANDLE, + CL_MSG_RING_GET, + CL_MSG_ERROR, +}; + +struct udebug_client_msg { + uint8_t type; + uint8_t _pad[3]; + uint32_t id; + union { + struct { + uint32_t ring_size, data_size; + }; + uint32_t notify_mask; + }; +} __attribute__((packed, aligned(4))); + +static inline struct udebug_ptr * +udebug_ring_ptr(struct udebug_hdr *hdr, uint32_t idx) +{ + struct udebug_ptr *ring = (struct udebug_ptr *)&hdr[1]; + return &ring[idx & (hdr->ring_size - 1)]; +} + +static inline void *udebug_buf_ptr(struct udebug_buf *buf, uint32_t ofs) +{ + return buf->data + (ofs & (buf->data_size - 1)); +} + +#endif diff --git a/udebug-remote.c b/udebug-remote.c new file mode 100644 index 0000000..f13f633 --- /dev/null +++ b/udebug-remote.c @@ -0,0 +1,382 @@ +/* + * udebug - debug ring buffer library + * + * Copyright (C) 2023 Felix Fietkau + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + */ +#include "udebug-priv.h" + +static struct udebug_client_msg * +send_and_wait(struct udebug *ctx, struct udebug_client_msg *msg, int *rfd) +{ + int type = msg->type; + int fd = -1; + + udebug_send_msg(ctx, msg, NULL, -1); + + do { + if (fd >= 0) + close(fd); + fd = -1; + msg = __udebug_poll(ctx, &fd, true); + } while (msg && msg->type != type); + if (!msg) + return NULL; + + if (rfd) + *rfd = fd; + else if (fd >= 0) + close(fd); + + return msg; +} + +static int +udebug_remote_get_handle(struct udebug *ctx) +{ + struct udebug_client_msg *msg; + struct udebug_client_msg send_msg = { + .type = CL_MSG_GET_HANDLE, + }; + + if (ctx->poll_handle >= 0 || !udebug_is_connected(ctx)) + return 0; + + msg = send_and_wait(ctx, &send_msg, NULL); + if (!msg) + return -1; + + ctx->poll_handle = msg->id; + return 0; +} + +struct udebug_remote_buf *udebug_remote_buf_get(struct udebug *ctx, uint32_t id) +{ + struct udebug_remote_buf *rb; + void *key = (void *)(uintptr_t)id; + + return avl_find_element(&ctx->remote_rings, key, rb, node); +} + +int udebug_remote_buf_map(struct udebug *ctx, struct udebug_remote_buf *rb, uint32_t id) +{ + void *key = (void *)(uintptr_t)id; + struct udebug_client_msg *msg; + struct udebug_client_msg send_msg = { + .type = CL_MSG_RING_GET, + .id = id, + }; + int fd = -1; + + if (rb->buf.data || !udebug_is_connected(ctx)) + return -1; + + msg = send_and_wait(ctx, &send_msg, &fd); + if (!msg || fd < 0) + return -1; + + if (udebug_buf_open(&rb->buf, fd, msg->ring_size, msg->data_size)) { + fprintf(stderr, "failed to open fd %d, ring_size=%d, data_size=%d\n", fd, msg->ring_size, msg->data_size); + close(fd); + return -1; + } + + rb->pcap_iface = ~0; + rb->node.key = key; + avl_insert(&ctx->remote_rings, &rb->node); + + return 0; +} + +void udebug_remote_buf_unmap(struct udebug *ctx, struct udebug_remote_buf *rb) +{ + if (!rb->buf.data) + return; + + avl_delete(&ctx->remote_rings, &rb->node); + udebug_buf_free(&rb->buf); + rb->poll = 0; + rb->node.key = NULL; + rb->pcap_iface = ~0; +} + +int udebug_remote_buf_set_poll(struct udebug *ctx, struct udebug_remote_buf *rb, bool val) +{ + int handle; + + if (!rb->buf.data) + return -1; + + if (rb->poll == val) + return 0; + + rb->poll = val; + if (!val) + return 0; + + handle = udebug_remote_get_handle(ctx); + if (handle < 0) + return -1; + + __atomic_fetch_or(&rb->buf.hdr->notify, 1UL << handle, __ATOMIC_RELAXED); + return 0; +} + +static void +rbuf_advance_read_head(struct udebug_remote_buf *rb, uint32_t head, + uint32_t *data_start) +{ + struct udebug_hdr *hdr = rb->buf.hdr; + uint32_t min_head = head + 1 - rb->buf.ring_size; + uint32_t min_data = u32_get(&hdr->data_used) - rb->buf.data_size; + struct udebug_ptr *last_ptr = udebug_ring_ptr(hdr, head - 1); + + if (!u32_get(&hdr->head_hi) && u32_sub(0, min_head) > 0) + min_head = 0; + + /* advance head to skip over any entries that are guaranteed + * to be overwritten now. final check will be performed after + * data copying */ + + if (u32_sub(rb->head, min_head) < 0) + rb->head = min_head; + + for (size_t i = 0; i < rb->buf.ring_size; i++) { + struct udebug_ptr *ptr = udebug_ring_ptr(hdr, rb->head); + + if (data_start) { + *data_start = u32_get(&ptr->start); + __sync_synchronize(); + } + + if (ptr->timestamp > last_ptr->timestamp) + continue; + + if (u32_sub(ptr->start, min_data) > 0) + break; + + rb->head++; + } +} + +void udebug_remote_buf_set_start_time(struct udebug_remote_buf *rb, uint64_t ts) +{ + struct udebug_hdr *hdr = rb->buf.hdr; + uint32_t head = u32_get(&hdr->head); + uint32_t start = rb->head, end = head; + uint32_t diff; + + if (!hdr) + return; + + rbuf_advance_read_head(rb, head, NULL); + while ((diff = u32_sub(end, start)) > 0) { + uint32_t cur = start + diff / 2; + struct udebug_ptr *ptr; + + ptr = udebug_ring_ptr(hdr, cur); + if (ptr->timestamp > ts) + end = cur - 1; + else + start = cur + 1; + } + + rb->head = start; +} + +void udebug_remote_buf_set_start_offset(struct udebug_remote_buf *rb, uint32_t idx) +{ + if (!rb->buf.hdr) + return; + + rb->head = rb->buf.hdr->head - idx; +} + +void udebug_remote_buf_set_flags(struct udebug_remote_buf *rb, uint64_t mask, uint64_t set) +{ + struct udebug_hdr *hdr = rb->buf.hdr; + + if (!hdr) + return; + + if ((uintptr_t)mask) + __atomic_and_fetch(&hdr->flags[0], (uintptr_t)~mask, __ATOMIC_RELAXED); + if ((uintptr_t)set) + __atomic_or_fetch(&hdr->flags[0], (uintptr_t)set, __ATOMIC_RELAXED); + + if (sizeof(mask) == sizeof(unsigned long)) + return; + + mask >>= 32; + if ((uintptr_t)mask) + __atomic_and_fetch(&hdr->flags[1], (uintptr_t)~mask, __ATOMIC_RELAXED); + if ((uintptr_t)set) + __atomic_or_fetch(&hdr->flags[1], (uintptr_t)set, __ATOMIC_RELAXED); +} + +struct udebug_snapshot * +udebug_remote_buf_snapshot(struct udebug_remote_buf *rb) +{ + struct udebug_hdr *hdr = rb->buf.hdr; + struct udebug_ptr *last_ptr; + uint32_t data_start, data_end, data_used; + struct udebug_snapshot *s = NULL; + struct udebug_ptr *ptr_buf, *first_ptr; + uint32_t data_size, ptr_size; + uint32_t head, first_idx; + uint32_t prev_read_head = rb->head; + void *data_buf; + + if (!hdr) + return NULL; + + head = u32_get(&hdr->head); + rbuf_advance_read_head(rb, head, &data_start); + if (rb->head == head) + return NULL; + + first_idx = rb->head; + first_ptr = udebug_ring_ptr(hdr, first_idx); + last_ptr = udebug_ring_ptr(hdr, head - 1); + data_end = last_ptr->start + last_ptr->len; + + data_size = data_end - data_start; + ptr_size = head - rb->head; + if (data_size > rb->buf.data_size || ptr_size > rb->buf.ring_size) { + fprintf(stderr, "Invalid data size: %x > %x, %x > %x\n", data_size, (int)rb->buf.data_size, ptr_size, (int)rb->buf.ring_size); + goto out; + } + + s = calloc_a(sizeof(*s), + &ptr_buf, ptr_size * sizeof(*ptr_buf), + &data_buf, data_size); + + s->data = memcpy(data_buf, udebug_buf_ptr(&rb->buf, data_start), data_size); + s->data_size = data_size; + s->entries = ptr_buf; + s->dropped = rb->head - prev_read_head; + + if (first_ptr > last_ptr) { + struct udebug_ptr *start_ptr = udebug_ring_ptr(hdr, 0); + struct udebug_ptr *end_ptr = udebug_ring_ptr(hdr, rb->buf.ring_size - 1) + 1; + uint32_t size = end_ptr - first_ptr; + memcpy(s->entries, first_ptr, size * sizeof(*s->entries)); + memcpy(s->entries + size, start_ptr, (last_ptr + 1 - start_ptr) * sizeof(*s->entries)); + } else { + memcpy(s->entries, first_ptr, (last_ptr + 1 - first_ptr) * sizeof(*s->entries)); + } + + /* get a snapshot of the counter that indicates how much data has been + * clobbered by newly added entries */ + __sync_synchronize(); + data_used = u32_get(&hdr->data_used) - rb->buf.data_size; + + s->n_entries = head - first_idx; + + rbuf_advance_read_head(rb, head, NULL); + if (s->n_entries < rb->head - first_idx) { + free(s); + s = NULL; + goto out; + } + + s->entries += rb->head - first_idx; + s->n_entries -= rb->head - first_idx; + while (s->n_entries > 0 && + u32_sub(s->entries[0].start, data_used) < 0) { + s->entries++; + s->n_entries--; + s->dropped++; + } + + for (size_t i = 0; i < s->n_entries; i++) + s->entries[i].start -= data_start; + + s->format = hdr->format; + s->sub_format = hdr->sub_format; + s->rbuf_idx = (uint32_t)(uintptr_t)rb->node.key; + +out: + rb->head = head; + return s; +} + +bool udebug_snapshot_get_entry(struct udebug_snapshot *s, struct udebug_iter *it, unsigned int entry) +{ + struct udebug_ptr *ptr; + + it->len = 0; + if (entry >= s->n_entries) + goto error; + + ptr = &s->entries[entry]; + if (ptr->start > s->data_size || ptr->len > s->data_size || + ptr->start + ptr->len > s->data_size) + goto error; + + it->s = s; + it->data = s->data + ptr->start; + it->len = ptr->len; + it->timestamp = ptr->timestamp; + return true; + +error: + it->data = NULL; + return false; +} + +void udebug_iter_start(struct udebug_iter *it, struct udebug_snapshot **s, size_t n) +{ + memset(it, 0, sizeof(*it)); + + it->list = s; + it->n = n; + + for (size_t i = 0; i < it->n; i++) + it->list[i]->iter_idx = 0; +} + +bool udebug_iter_next(struct udebug_iter *it) +{ + while (1) { + struct udebug_snapshot *s; + uint64_t cur_ts; + int cur = -1; + + for (size_t i = 0; i < it->n; i++) { + struct udebug_ptr *ptr; + + s = it->list[i]; + if (s->iter_idx >= s->n_entries) + continue; + + ptr = &s->entries[s->iter_idx]; + if (cur >= 0 && ptr->timestamp > cur_ts) + continue; + + cur = i; + cur_ts = ptr->timestamp; + } + + if (cur < 0) + return false; + + s = it->list[cur]; + it->s_idx = cur; + if (!udebug_snapshot_get_entry(s, it, s->iter_idx++)) + continue; + + return true; + } +} diff --git a/udebug.c b/udebug.c new file mode 100644 index 0000000..5be9b54 --- /dev/null +++ b/udebug.c @@ -0,0 +1,758 @@ +/* + * udebug - debug ring buffer library + * + * Copyright (C) 2023 Felix Fietkau + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + */ +#define _GNU_SOURCE +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "udebug-priv.h" +#include "usock.h" + +#define ALIGN(i, sz) (((i) + (sz) - 1) & ~((sz) - 1)) + +#ifndef MAP_ANONYMOUS +#define MAP_ANONYMOUS MAP_ANON +#endif + +#define UDEBUG_MIN_ALLOC_LEN 128 +static struct blob_buf b; + +static void __randname(char *template) +{ + int i; + struct timespec ts; + unsigned long r; + + clock_gettime(CLOCK_REALTIME, &ts); + r = ts.tv_sec + ts.tv_nsec; + for (i=0; i<6; i++, r>>=5) + template[i] = 'A'+(r&15)+(r&16)*2; +} + +int udebug_id_cmp(const void *k1, const void *k2, void *ptr) +{ + uint32_t id1 = (uint32_t)(uintptr_t)k1, id2 = (uint32_t)(uintptr_t)k2; + if (id1 < id2) + return -1; + else if (id1 > id2) + return 1; + return 0; +} + +static inline int +shm_open_anon(char *name) +{ + char *template = name + strlen(name) - 6; + int fd; + + if (template < name || memcmp(template, "XXXXXX", 6) != 0) + return -1; + + for (int i = 0; i < 100; i++) { + __randname(template); + fd = shm_open(name, O_RDWR | O_CREAT | O_EXCL, 0600); + if (fd >= 0) { + if (shm_unlink(name) < 0) { + close(fd); + continue; + } + return fd; + } + + if (fd < 0 && errno != EEXIST) + return -1; + } + + return -1; +} + +static void __udebug_disconnect(struct udebug *ctx, bool reconnect) +{ + uloop_fd_delete(&ctx->fd); + close(ctx->fd.fd); + ctx->fd.fd = -1; + ctx->poll_handle = -1; + if (ctx->reconnect.cb && reconnect) + uloop_timeout_set(&ctx->reconnect, 1); +} + +uint64_t udebug_timestamp(void) +{ + struct timespec ts; + uint64_t val; + + clock_gettime(CLOCK_REALTIME, &ts); + + val = ts.tv_sec; + val *= UDEBUG_TS_SEC; + val += ts.tv_nsec / 1000; + + return val; +} + +static int +__udebug_buf_map(struct udebug_buf *buf) +{ + void *ptr, *ptr2; + + ptr = mmap(NULL, buf->head_size + 2 * buf->data_size, PROT_NONE, + MAP_ANONYMOUS | MAP_PRIVATE, -1, 0); + if (ptr == MAP_FAILED) + return -1; + + ptr2 = mmap(ptr, buf->head_size + buf->data_size, + PROT_READ | PROT_WRITE, MAP_FIXED | MAP_SHARED, buf->fd, 0); + if (ptr2 != ptr) + goto err_unmap; + + ptr2 = mmap(ptr + buf->head_size + buf->data_size, buf->data_size, + PROT_READ | PROT_WRITE, MAP_FIXED | MAP_SHARED, buf->fd, + buf->head_size); + if (ptr2 != ptr + buf->head_size + buf->data_size) + goto err_unmap; + + buf->hdr = ptr; + buf->data = ptr + buf->head_size; + return 0; + +err_unmap: + munmap(ptr, buf->head_size + 2 * buf->data_size); + return -1; +} + +static int +writev_retry(int fd, struct iovec *iov, int iov_len, int sock_fd) +{ + uint8_t fd_buf[CMSG_SPACE(sizeof(int))] = { 0 }; + struct msghdr msghdr = { 0 }; + struct cmsghdr *cmsg; + int len = 0; + int *pfd; + + msghdr.msg_iov = iov, + msghdr.msg_iovlen = iov_len, + msghdr.msg_control = fd_buf; + msghdr.msg_controllen = sizeof(fd_buf); + + cmsg = CMSG_FIRSTHDR(&msghdr); + cmsg->cmsg_type = SCM_RIGHTS; + cmsg->cmsg_level = SOL_SOCKET; + cmsg->cmsg_len = CMSG_LEN(sizeof(int)); + + pfd = (int *) CMSG_DATA(cmsg); + msghdr.msg_controllen = cmsg->cmsg_len; + + do { + ssize_t cur_len; + + if (sock_fd < 0) { + msghdr.msg_control = NULL; + msghdr.msg_controllen = 0; + } else { + *pfd = sock_fd; + } + + cur_len = sendmsg(fd, &msghdr, 0); + if (cur_len < 0) { + struct pollfd pfd = { + .fd = fd, + .events = POLLOUT + }; + + switch(errno) { + case EAGAIN: + poll(&pfd, 1, -1); + break; + case EINTR: + break; + default: + return -1; + } + continue; + } + + if (len > 0) + sock_fd = -1; + + len += cur_len; + while (cur_len >= (ssize_t) iov->iov_len) { + cur_len -= iov->iov_len; + iov_len--; + iov++; + if (!iov_len) + return len; + } + iov->iov_base += cur_len; + iov->iov_len -= cur_len; + msghdr.msg_iov = iov; + msghdr.msg_iovlen = iov_len; + } while (1); + + /* Should never reach here */ + return -1; +} + +static int +recv_retry(int fd, struct iovec *iov, bool wait, int *recv_fd) +{ + uint8_t fd_buf[CMSG_SPACE(sizeof(int))] = { 0 }; + struct msghdr msghdr = { 0 }; + struct cmsghdr *cmsg; + int total = 0; + int bytes; + int *pfd; + + msghdr.msg_iov = iov, + msghdr.msg_iovlen = 1, + msghdr.msg_control = fd_buf; + msghdr.msg_controllen = sizeof(fd_buf); + + cmsg = CMSG_FIRSTHDR(&msghdr); + cmsg->cmsg_type = SCM_RIGHTS; + cmsg->cmsg_level = SOL_SOCKET; + cmsg->cmsg_len = CMSG_LEN(sizeof(int)); + + pfd = (int *) CMSG_DATA(cmsg); + + while (iov->iov_len > 0) { + if (recv_fd) { + msghdr.msg_control = fd_buf; + msghdr.msg_controllen = cmsg->cmsg_len; + } else { + msghdr.msg_control = NULL; + msghdr.msg_controllen = 0; + } + + *pfd = -1; + bytes = recvmsg(fd, &msghdr, 0); + if (!bytes) + return -2; + if (bytes < 0) { + bytes = 0; + if (errno == EINTR) + continue; + + if (errno != EAGAIN) + return -2; + } + if (!wait && !bytes) + return 0; + + if (recv_fd) + *recv_fd = *pfd; + else if (*pfd >= 0) + close(*pfd); + + if (bytes > 0) + recv_fd = NULL; + + wait = true; + iov->iov_len -= bytes; + iov->iov_base += bytes; + total += bytes; + + if (iov->iov_len > 0) { + struct pollfd pfd = { + .fd = fd, + .events = POLLIN + }; + int ret; + do { + ret = poll(&pfd, 1, UDEBUG_TIMEOUT); + } while (ret < 0 && errno == EINTR); + + if (!(pfd.revents & POLLIN)) + return -1; + } + } + + return total; +} + +void udebug_send_msg(struct udebug *ctx, struct udebug_client_msg *msg, + struct blob_attr *meta, int fd) +{ + struct iovec iov[2] = { + { .iov_base = msg, .iov_len = sizeof(*msg) }, + {} + }; + + if (!meta) { + blob_buf_init(&b, 0); + meta = b.head; + } + + iov[1].iov_base = meta; + iov[1].iov_len = blob_pad_len(meta); + writev_retry(ctx->fd.fd, iov, ARRAY_SIZE(iov), fd); +} + +static void +udebug_buf_msg(struct udebug_buf *buf, enum udebug_client_msg_type type) +{ + struct udebug_client_msg msg = { + .type = type, + .id = buf->id, + }; + + udebug_send_msg(buf->ctx, &msg, NULL, -1); +} + +static size_t __udebug_headsize(unsigned int ring_size, unsigned int page_size) +{ + ring_size *= sizeof(struct udebug_ptr); + return ALIGN(sizeof(struct udebug_hdr) + ring_size, page_size); +} + +int udebug_buf_open(struct udebug_buf *buf, int fd, uint32_t ring_size, uint32_t data_size) +{ + INIT_LIST_HEAD(&buf->list); + buf->fd = fd; + buf->ring_size = ring_size; + buf->head_size = __udebug_headsize(ring_size, sysconf(_SC_PAGESIZE)); + buf->data_size = data_size; + + if (buf->ring_size > (1U << 24) || buf->data_size > (1U << 29)) + return -1; + + if (__udebug_buf_map(buf)) + return -1; + + if (buf->ring_size != buf->hdr->ring_size || + buf->data_size != buf->hdr->data_size) { + munmap(buf->hdr, buf->head_size + 2 * buf->data_size); + buf->hdr = NULL; + return -1; + } + + return 0; +} + +int udebug_buf_init(struct udebug_buf *buf, size_t entries, size_t size) +{ + uint32_t pagesz = sysconf(_SC_PAGESIZE); + char filename[] = "/udebug.XXXXXX"; + unsigned int order = 12; + uint8_t ring_order = 5; + size_t head_size; + int fd; + + INIT_LIST_HEAD(&buf->list); + if (size < pagesz) + size = pagesz; + while(size > 1U << order) + order++; + size = 1 << order; + while (entries > 1U << ring_order) + ring_order++; + entries = 1 << ring_order; + + if (size > (1U << 29) || entries > (1U << 24)) + return -1; + + head_size = __udebug_headsize(entries, pagesz); + while (ALIGN(sizeof(*buf->hdr) + (entries * 2) * sizeof(struct udebug_ptr), pagesz) == head_size) + entries *= 2; + + fd = shm_open_anon(filename); + if (fd < 0) + return -1; + + if (ftruncate(fd, head_size + size) < 0) + goto err_close; + + buf->head_size = head_size; + buf->data_size = size; + buf->ring_size = entries; + buf->fd = fd; + + if (__udebug_buf_map(buf)) + goto err_close; + + buf->hdr->ring_size = entries; + buf->hdr->data_size = size; + + /* ensure hdr changes are visible */ + __sync_synchronize(); + + return 0; + +err_close: + close(fd); + return -1; +} + +static void *udebug_buf_alloc(struct udebug_buf *buf, uint32_t ofs, uint32_t len) +{ + struct udebug_hdr *hdr = buf->hdr; + + hdr->data_used = u32_max(hdr->data_used, ofs + len + 1); + + /* ensure that data_used update is visible before clobbering data */ + __sync_synchronize(); + + return udebug_buf_ptr(buf, ofs); +} + +uint64_t udebug_buf_flags(struct udebug_buf *buf) +{ + struct udebug_hdr *hdr = buf->hdr; + uint64_t flags; + + if (!hdr) + return 0; + + flags = hdr->flags[0]; + if (sizeof(flags) != sizeof(uintptr_t)) + flags |= ((uint64_t)hdr->flags[1]) << 32; + + return flags; +} + +void udebug_entry_init_ts(struct udebug_buf *buf, uint64_t timestamp) +{ + struct udebug_hdr *hdr = buf->hdr; + struct udebug_ptr *ptr; + + if (!hdr) + return; + + ptr = udebug_ring_ptr(hdr, hdr->head); + ptr->start = hdr->data_head; + ptr->len = 0; + ptr->timestamp = timestamp; +} + +void *udebug_entry_append(struct udebug_buf *buf, const void *data, uint32_t len) +{ + struct udebug_hdr *hdr = buf->hdr; + struct udebug_ptr *ptr; + uint32_t ofs; + void *ret; + + if (!hdr) + return NULL; + + ptr = udebug_ring_ptr(hdr, hdr->head); + ofs = ptr->start + ptr->len; + if (ptr->len + len > buf->data_size / 2) + return NULL; + + ret = udebug_buf_alloc(buf, ofs, len); + if (data) + memcpy(ret, data, len); + ptr->len += len; + + return ret; +} + +int udebug_entry_printf(struct udebug_buf *buf, const char *fmt, ...) +{ + va_list ap; + size_t ret; + + va_start(ap, fmt); + ret = udebug_entry_vprintf(buf, fmt, ap); + va_end(ap); + + return ret; +} + +int udebug_entry_vprintf(struct udebug_buf *buf, const char *fmt, va_list ap) +{ + struct udebug_hdr *hdr = buf->hdr; + struct udebug_ptr *ptr; + uint32_t ofs; + uint32_t len; + char *str; + + if (!hdr) + return -1; + + ptr = udebug_ring_ptr(hdr, hdr->head); + ofs = ptr->start + ptr->len; + if (ptr->len > buf->data_size / 2) + return -1; + + str = udebug_buf_alloc(buf, ofs, UDEBUG_MIN_ALLOC_LEN); + len = vsnprintf(str, UDEBUG_MIN_ALLOC_LEN, fmt, ap); + if (len <= UDEBUG_MIN_ALLOC_LEN) + goto out; + + if (ptr->len + len > buf->data_size / 2) + return -1; + + udebug_buf_alloc(buf, ofs, len + 1); + len = vsnprintf(str, len, fmt, ap); + +out: + ptr->len += len; + return 0; +} + +void udebug_entry_add(struct udebug_buf *buf) +{ + struct udebug_hdr *hdr = buf->hdr; + struct udebug_ptr *ptr = udebug_ring_ptr(hdr, hdr->head); + uint32_t notify; + uint8_t *data; + + /* ensure strings are always 0-terminated */ + data = udebug_buf_ptr(buf, ptr->start + ptr->len); + *data = 0; + hdr->data_head = ptr->start + ptr->len + 1; + + /* ensure that all data changes are visible before advancing head */ + __sync_synchronize(); + + u32_set(&hdr->head, u32_get(&hdr->head) + 1); + if (!u32_get(&hdr->head)) + u32_set(&hdr->head_hi, u32_get(&hdr->head_hi) + 1); + + /* ensure that head change is visible */ + __sync_synchronize(); + + notify = __atomic_exchange_n(&hdr->notify, 0, __ATOMIC_RELAXED); + if (notify) { + struct udebug_client_msg msg = { + .type = CL_MSG_RING_NOTIFY, + .id = buf->id, + .notify_mask = notify, + }; + blob_buf_init(&b, 0); + + udebug_send_msg(buf->ctx, &msg, b.head, -1); + } +} +void udebug_buf_free(struct udebug_buf *buf) +{ + struct udebug *ctx = buf->ctx; + + if (!list_empty(&buf->list) && buf->list.prev) + list_del(&buf->list); + + if (ctx && ctx->fd.fd >= 0) + udebug_buf_msg(buf, CL_MSG_RING_REMOVE); + + munmap(buf->hdr, buf->head_size + 2 * buf->data_size); + close(buf->fd); + memset(buf, 0, sizeof(*buf)); +} + +static void +__udebug_buf_add(struct udebug *ctx, struct udebug_buf *buf) +{ + struct udebug_client_msg msg = { + .type = CL_MSG_RING_ADD, + .id = buf->id, + .ring_size = buf->hdr->ring_size, + .data_size = buf->hdr->data_size, + }; + const struct udebug_buf_meta *meta = buf->meta; + void *c; + + blob_buf_init(&b, 0); + blobmsg_add_string(&b, "name", meta->name); + c = blobmsg_open_array(&b, "flags"); + for (size_t i = 0; i < meta->n_flags; i++) { + const struct udebug_buf_flag *flag = &meta->flags[i]; + void *e = blobmsg_open_array(&b, NULL); + blobmsg_add_string(&b, NULL, flag->name); + blobmsg_add_u64(&b, NULL, flag->mask); + blobmsg_close_array(&b, e); + } + blobmsg_close_array(&b, c); + + udebug_send_msg(ctx, &msg, b.head, buf->fd); +} + +int udebug_buf_add(struct udebug *ctx, struct udebug_buf *buf, + const struct udebug_buf_meta *meta) +{ + list_add_tail(&buf->list, &ctx->local_rings); + buf->ctx = ctx; + buf->meta = meta; + buf->id = ctx->next_id++; + buf->hdr->format = meta->format; + buf->hdr->sub_format = meta->sub_format; + + if (ctx->fd.fd >= 0) + __udebug_buf_add(ctx, buf); + + return 0; +} + +void udebug_init(struct udebug *ctx) +{ + INIT_LIST_HEAD(&ctx->local_rings); + avl_init(&ctx->remote_rings, udebug_id_cmp, true, NULL); + ctx->fd.fd = -1; + ctx->poll_handle = -1; +} + +static void udebug_reconnect_cb(struct uloop_timeout *t) +{ + struct udebug *ctx = container_of(t, struct udebug, reconnect); + + if (udebug_connect(ctx, ctx->socket_path) < 0) { + uloop_timeout_set(&ctx->reconnect, 1000); + return; + } + + udebug_add_uloop(ctx); +} + +void udebug_auto_connect(struct udebug *ctx, const char *path) +{ + free(ctx->socket_path); + ctx->reconnect.cb = udebug_reconnect_cb; + ctx->socket_path = path ? strdup(path) : NULL; + if (ctx->fd.fd >= 0) + return; + + udebug_reconnect_cb(&ctx->reconnect); +} + +int udebug_connect(struct udebug *ctx, const char *path) +{ + struct udebug_remote_buf *rb; + struct udebug_buf *buf; + + if (ctx->fd.fd >= 0) + close(ctx->fd.fd); + ctx->fd.fd = -1; + + if (!path) + path = UDEBUG_SOCK_NAME; + + ctx->fd.fd = usock(USOCK_UNIX, path, NULL); + if (ctx->fd.fd < 0) + return -1; + + list_for_each_entry(buf, &ctx->local_rings, list) + __udebug_buf_add(ctx, buf); + + avl_for_each_element(&ctx->remote_rings, rb, node) { + if (!rb->poll) + continue; + + rb->poll = false; + udebug_remote_buf_set_poll(ctx, rb, true); + } + + return 0; +} + +static bool +udebug_recv_msg(struct udebug *ctx, struct udebug_client_msg *msg, int *fd, + bool wait) +{ + struct iovec iov = { + .iov_base = msg, + .iov_len = sizeof(*msg) + }; + int ret; + + ret = recv_retry(ctx->fd.fd, &iov, wait, fd); + if (ret == -2) + __udebug_disconnect(ctx, true); + + return ret == sizeof(*msg); +} + +struct udebug_client_msg *__udebug_poll(struct udebug *ctx, int *fd, bool wait) +{ + static struct udebug_client_msg msg = {}; + + while (udebug_recv_msg(ctx, &msg, fd, wait)) { + struct udebug_remote_buf *rb; + void *key; + + if (msg.type != CL_MSG_RING_NOTIFY) + return &msg; + + if (fd && *fd >= 0) + close(*fd); + + if (!ctx->notify_cb) + continue; + + key = (void *)(uintptr_t)msg.id; + rb = avl_find_element(&ctx->remote_rings, key, rb, node); + if (!rb || !rb->poll) + continue; + + if (ctx->poll_handle >= 0) + __atomic_fetch_or(&rb->buf.hdr->notify, + 1UL << ctx->poll_handle, + __ATOMIC_RELAXED); + ctx->notify_cb(ctx, rb); + } + + return NULL; +} + +void udebug_poll(struct udebug *ctx) +{ + while (__udebug_poll(ctx, NULL, false)); +} + +static void udebug_fd_cb(struct uloop_fd *fd, unsigned int events) +{ + struct udebug *ctx = container_of(fd, struct udebug, fd); + + if (fd->eof) + __udebug_disconnect(ctx, true); + + udebug_poll(ctx); +} + +void udebug_add_uloop(struct udebug *ctx) +{ + if (ctx->fd.registered) + return; + + ctx->fd.cb = udebug_fd_cb; + uloop_fd_add(&ctx->fd, ULOOP_READ); +} + +void udebug_free(struct udebug *ctx) +{ + struct udebug_remote_buf *rb, *tmp; + struct udebug_buf *buf; + + free(ctx->socket_path); + ctx->socket_path = NULL; + + __udebug_disconnect(ctx, false); + uloop_timeout_cancel(&ctx->reconnect); + + while (!list_empty(&ctx->local_rings)) { + buf = list_first_entry(&ctx->local_rings, struct udebug_buf, list); + udebug_buf_free(buf); + } + + avl_for_each_element_safe(&ctx->remote_rings, rb, node, tmp) + udebug_remote_buf_unmap(ctx, rb); +} diff --git a/udebug.h b/udebug.h new file mode 100644 index 0000000..d24dbfc --- /dev/null +++ b/udebug.h @@ -0,0 +1,196 @@ +/* + * udebug - debug ring buffer library + * + * Copyright (C) 2023 Felix Fietkau + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + */ +#ifndef __UDEBUG_RINGBUF_H +#define __UDEBUG_RINGBUF_H + +#include +#include +#include + +#include "list.h" +#include "uloop.h" +#include "avl.h" + +#define UDEBUG_SOCK_NAME "/var/run/udebug.sock" + +enum udebug_format { + UDEBUG_FORMAT_PACKET, + UDEBUG_FORMAT_STRING, + UDEBUG_FORMAT_BLOBMSG, +}; + +enum { + UDEBUG_DLT_ETHERNET = 1, + UDEBUG_DLT_PPP = 50, + UDEBUG_DLT_RAW_IP = 101, + UDEBUG_DLT_IEEE_802_11 = 105, + UDEBUG_DLT_IEEE_802_11_RADIOTAP = 127, + UDEBUG_DLT_NETLINK = 253, +}; + +enum udebug_meta_type { + UDEBUG_META_IFACE_NAME, + UDEBUG_META_IFACE_DESC, + __UDEBUG_META_MAX +}; + +#define UDEBUG_TS_MSEC 1000ULL +#define UDEBUG_TS_SEC (1000ULL * UDEBUG_TS_MSEC) + +struct udebug; +struct udebug_hdr; + +struct udebug_buf_flag { + const char *name; + uint64_t mask; +}; + +struct udebug_buf_meta { + const char *name; + enum udebug_format format; + uint32_t sub_format; /* linktype for UDEBUG_FORMAT_PACKET */ + const struct udebug_buf_flag *flags; + unsigned int n_flags; +}; + +struct udebug_buf { + struct udebug *ctx; + const struct udebug_buf_meta *meta; + uint32_t id; + + struct list_head list; + + struct udebug_hdr *hdr; + void *data; + size_t data_size; + size_t head_size; + size_t ring_size; + int fd; +}; + +struct udebug_packet_info { + const char *attr[__UDEBUG_META_MAX]; +}; + +struct udebug_remote_buf { + struct avl_node node; + struct udebug_buf buf; + bool poll; + uint32_t head; + + /* provided by user */ + uint32_t pcap_iface; + void *priv; + const struct udebug_packet_info *meta; +}; + +struct udebug { + struct list_head local_rings; + struct avl_tree remote_rings; + uint32_t next_id; + struct uloop_fd fd; + int poll_handle; + char *socket_path; + struct uloop_timeout reconnect; + + /* filled by user */ + void (*notify_cb)(struct udebug *ctx, struct udebug_remote_buf *rb); +}; + +struct udebug_ptr { + uint32_t start; + uint32_t len; + uint64_t timestamp; +}; + +struct udebug_snapshot { + struct udebug_ptr *entries; + unsigned int n_entries; + unsigned int dropped; + void *data; + size_t data_size; + + uint32_t iter_idx; + + enum udebug_format format; + uint32_t sub_format; + + uint32_t rbuf_idx; +}; + +struct udebug_iter { + struct udebug_snapshot **list; + size_t n; + + struct udebug_snapshot *s; + unsigned int s_idx; + + uint64_t timestamp; + void *data; + size_t len; +}; + +uint64_t udebug_timestamp(void); + +void udebug_entry_init_ts(struct udebug_buf *buf, uint64_t timestamp); +static inline void udebug_entry_init(struct udebug_buf *buf) +{ + udebug_entry_init_ts(buf, udebug_timestamp()); +} +void *udebug_entry_append(struct udebug_buf *buf, const void *data, uint32_t len); +int udebug_entry_printf(struct udebug_buf *buf, const char *fmt, ...) + __attribute__ ((format (printf, 2, 3))); +int udebug_entry_vprintf(struct udebug_buf *buf, const char *fmt, va_list ap) + __attribute__ ((format (printf, 2, 0))); +void udebug_entry_add(struct udebug_buf *buf); + +int udebug_buf_init(struct udebug_buf *buf, size_t entries, size_t size); +int udebug_buf_add(struct udebug *ctx, struct udebug_buf *buf, + const struct udebug_buf_meta *meta); +uint64_t udebug_buf_flags(struct udebug_buf *buf); +void udebug_buf_free(struct udebug_buf *buf); + +struct udebug_remote_buf *udebug_remote_buf_get(struct udebug *ctx, uint32_t id); +int udebug_remote_buf_map(struct udebug *ctx, struct udebug_remote_buf *rb, uint32_t id); +void udebug_remote_buf_unmap(struct udebug *ctx, struct udebug_remote_buf *rb); +int udebug_remote_buf_set_poll(struct udebug *ctx, struct udebug_remote_buf *rb, bool val); +void udebug_remote_buf_set_flags(struct udebug_remote_buf *rb, uint64_t mask, uint64_t set); +struct udebug_snapshot *udebug_remote_buf_snapshot(struct udebug_remote_buf *rb); +bool udebug_snapshot_get_entry(struct udebug_snapshot *s, struct udebug_iter *it, unsigned int entry); + +void udebug_remote_buf_set_start_time(struct udebug_remote_buf *rb, uint64_t ts); +void udebug_remote_buf_set_start_offset(struct udebug_remote_buf *rb, uint32_t idx); + +void udebug_iter_start(struct udebug_iter *it, struct udebug_snapshot **s, size_t n); +bool udebug_iter_next(struct udebug_iter *it); + +void udebug_init(struct udebug *ctx); +int udebug_connect(struct udebug *ctx, const char *path); +void udebug_auto_connect(struct udebug *ctx, const char *path); +void udebug_add_uloop(struct udebug *ctx); +void udebug_poll(struct udebug *ctx); +void udebug_free(struct udebug *ctx); + +static inline bool udebug_is_connected(struct udebug *ctx) +{ + return ctx->fd.fd >= 0; +} + +int udebug_id_cmp(const void *k1, const void *k2, void *ptr); + +#endif