lib: unmap full ring buffer
[project/udebug.git] / lib.c
1 #define _GNU_SOURCE
2 #include <sys/types.h>
3 #include <sys/mman.h>
4 #include <sys/socket.h>
5 #include <unistd.h>
6 #include <string.h>
7 #include <stdio.h>
8 #include <fcntl.h>
9 #include <errno.h>
10 #include <poll.h>
11 #include <time.h>
12 #include "priv.h"
13
14 #include <libubox/usock.h>
15
16 #define ALIGN(i, sz) (((i) + (sz) - 1) & ~((sz) - 1))
17
18 #ifndef MAP_ANONYMOUS
19 #define MAP_ANONYMOUS MAP_ANON
20 #endif
21
22 #define UDEBUG_MIN_ALLOC_LEN 128
23 static struct blob_buf b;
24
25 static void __randname(char *template)
26 {
27 int i;
28 struct timespec ts;
29 unsigned long r;
30
31 clock_gettime(CLOCK_REALTIME, &ts);
32 r = ts.tv_sec + ts.tv_nsec;
33 for (i=0; i<6; i++, r>>=5)
34 template[i] = 'A'+(r&15)+(r&16)*2;
35 }
36
37 int udebug_id_cmp(const void *k1, const void *k2, void *ptr)
38 {
39 uint32_t id1 = (uint32_t)(uintptr_t)k1, id2 = (uint32_t)(uintptr_t)k2;
40 return id1 - id2;
41 }
42
43 static inline int
44 shm_open_anon(char *name)
45 {
46 char *template = name + strlen(name) - 6;
47 int fd;
48
49 if (template < name || memcmp(template, "XXXXXX", 6) != 0)
50 return -1;
51
52 for (int i = 0; i < 100; i++) {
53 __randname(template);
54 fd = shm_open(name, O_RDWR | O_CREAT | O_EXCL, 0600);
55 if (fd >= 0) {
56 if (shm_unlink(name) < 0) {
57 close(fd);
58 continue;
59 }
60 return fd;
61 }
62
63 if (fd < 0 && errno != EEXIST)
64 return -1;
65 }
66
67 return -1;
68 }
69
70 uint64_t udebug_timestamp(void)
71 {
72 struct timespec ts;
73 uint64_t val;
74
75 clock_gettime(CLOCK_REALTIME, &ts);
76
77 val = ts.tv_sec;
78 val *= UDEBUG_TS_SEC;
79 val += ts.tv_nsec / 1000;
80
81 return val;
82 }
83
84 static int
85 __udebug_buf_map(struct udebug_buf *buf)
86 {
87 void *ptr, *ptr2;
88
89 ptr = mmap(NULL, buf->head_size + 2 * buf->data_size, PROT_NONE,
90 MAP_ANONYMOUS | MAP_PRIVATE, -1, 0);
91 if (ptr == MAP_FAILED)
92 return -1;
93
94 ptr2 = mmap(ptr, buf->head_size + buf->data_size,
95 PROT_READ | PROT_WRITE, MAP_FIXED | MAP_SHARED, buf->fd, 0);
96 if (ptr2 != ptr)
97 goto err_unmap;
98
99 ptr2 = mmap(ptr + buf->head_size + buf->data_size, buf->data_size,
100 PROT_READ | PROT_WRITE, MAP_FIXED | MAP_SHARED, buf->fd,
101 buf->head_size);
102 if (ptr2 != ptr + buf->head_size + buf->data_size)
103 goto err_unmap;
104
105 buf->hdr = ptr;
106 buf->data = ptr + buf->head_size;
107 return 0;
108
109 err_unmap:
110 munmap(ptr, buf->head_size + 2 * buf->data_size);
111 return -1;
112 }
113
114 static int
115 writev_retry(int fd, struct iovec *iov, int iov_len, int sock_fd)
116 {
117 uint8_t fd_buf[CMSG_SPACE(sizeof(int))] = { 0 };
118 struct msghdr msghdr = { 0 };
119 struct cmsghdr *cmsg;
120 int len = 0;
121 int *pfd;
122
123 msghdr.msg_iov = iov,
124 msghdr.msg_iovlen = iov_len,
125 msghdr.msg_control = fd_buf;
126 msghdr.msg_controllen = sizeof(fd_buf);
127
128 cmsg = CMSG_FIRSTHDR(&msghdr);
129 cmsg->cmsg_type = SCM_RIGHTS;
130 cmsg->cmsg_level = SOL_SOCKET;
131 cmsg->cmsg_len = CMSG_LEN(sizeof(int));
132
133 pfd = (int *) CMSG_DATA(cmsg);
134 msghdr.msg_controllen = cmsg->cmsg_len;
135
136 do {
137 ssize_t cur_len;
138
139 if (sock_fd < 0) {
140 msghdr.msg_control = NULL;
141 msghdr.msg_controllen = 0;
142 } else {
143 *pfd = sock_fd;
144 }
145
146 cur_len = sendmsg(fd, &msghdr, 0);
147 if (cur_len < 0) {
148 struct pollfd pfd = {
149 .fd = fd,
150 .events = POLLOUT
151 };
152
153 switch(errno) {
154 case EAGAIN:
155 poll(&pfd, 1, -1);
156 break;
157 case EINTR:
158 break;
159 default:
160 return -1;
161 }
162 continue;
163 }
164
165 if (len > 0)
166 sock_fd = -1;
167
168 len += cur_len;
169 while (cur_len >= (ssize_t) iov->iov_len) {
170 cur_len -= iov->iov_len;
171 iov_len--;
172 iov++;
173 if (!iov_len)
174 return len;
175 }
176 iov->iov_base += cur_len;
177 iov->iov_len -= cur_len;
178 msghdr.msg_iov = iov;
179 msghdr.msg_iovlen = iov_len;
180 } while (1);
181
182 /* Should never reach here */
183 return -1;
184 }
185
186 static int
187 recv_retry(int fd, struct iovec *iov, bool wait, int *recv_fd)
188 {
189 uint8_t fd_buf[CMSG_SPACE(sizeof(int))] = { 0 };
190 struct msghdr msghdr = { 0 };
191 struct cmsghdr *cmsg;
192 int total = 0;
193 int bytes;
194 int *pfd;
195
196 msghdr.msg_iov = iov,
197 msghdr.msg_iovlen = 1,
198 msghdr.msg_control = fd_buf;
199 msghdr.msg_controllen = sizeof(fd_buf);
200
201 cmsg = CMSG_FIRSTHDR(&msghdr);
202 cmsg->cmsg_type = SCM_RIGHTS;
203 cmsg->cmsg_level = SOL_SOCKET;
204 cmsg->cmsg_len = CMSG_LEN(sizeof(int));
205
206 pfd = (int *) CMSG_DATA(cmsg);
207
208 while (iov->iov_len > 0) {
209 if (recv_fd) {
210 msghdr.msg_control = fd_buf;
211 msghdr.msg_controllen = cmsg->cmsg_len;
212 } else {
213 msghdr.msg_control = NULL;
214 msghdr.msg_controllen = 0;
215 }
216
217 *pfd = -1;
218 bytes = recvmsg(fd, &msghdr, 0);
219 if (!bytes)
220 return -2;
221 if (bytes < 0) {
222 bytes = 0;
223 if (errno == EINTR)
224 continue;
225
226 if (errno != EAGAIN)
227 return -2;
228 }
229 if (!wait && !bytes)
230 return 0;
231
232 if (recv_fd)
233 *recv_fd = *pfd;
234 else if (*pfd >= 0)
235 close(*pfd);
236
237 if (bytes > 0)
238 recv_fd = NULL;
239
240 wait = true;
241 iov->iov_len -= bytes;
242 iov->iov_base += bytes;
243 total += bytes;
244
245 if (iov->iov_len > 0) {
246 struct pollfd pfd = {
247 .fd = fd,
248 .events = POLLIN
249 };
250 int ret;
251 do {
252 ret = poll(&pfd, 1, UDEBUG_TIMEOUT);
253 } while (ret < 0 && errno == EINTR);
254
255 if (!(pfd.revents & POLLIN))
256 return -1;
257 }
258 }
259
260 return total;
261 }
262
263 void udebug_send_msg(struct udebug *ctx, struct udebug_client_msg *msg,
264 struct blob_attr *meta, int fd)
265 {
266 struct iovec iov[2] = {
267 { .iov_base = msg, .iov_len = sizeof(*msg) },
268 {}
269 };
270
271 if (!meta) {
272 blob_buf_init(&b, 0);
273 meta = b.head;
274 }
275
276 iov[1].iov_base = meta;
277 iov[1].iov_len = blob_pad_len(meta);
278 writev_retry(ctx->fd.fd, iov, ARRAY_SIZE(iov), fd);
279 }
280
281 static void
282 udebug_buf_msg(struct udebug_buf *buf, enum udebug_client_msg_type type)
283 {
284 struct udebug_client_msg msg = {
285 .type = type,
286 .id = buf->id,
287 };
288
289 udebug_send_msg(buf->ctx, &msg, NULL, -1);
290 }
291
292 static size_t __udebug_headsize(unsigned int ring_size, unsigned int page_size)
293 {
294 ring_size *= sizeof(struct udebug_ptr);
295 return ALIGN(sizeof(struct udebug_hdr) + ring_size, page_size);
296 }
297
298 int udebug_buf_open(struct udebug_buf *buf, int fd, uint32_t ring_size, uint32_t data_size)
299 {
300 INIT_LIST_HEAD(&buf->list);
301 buf->fd = fd;
302 buf->ring_size = ring_size;
303 buf->head_size = __udebug_headsize(ring_size, sysconf(_SC_PAGESIZE));
304 buf->data_size = data_size;
305
306 if (buf->ring_size > (1U << 24) || buf->data_size > (1U << 29))
307 return -1;
308
309 if (__udebug_buf_map(buf))
310 return -1;
311
312 if (buf->ring_size != buf->hdr->ring_size ||
313 buf->data_size != buf->hdr->data_size) {
314 munmap(buf->hdr, buf->head_size + 2 * buf->data_size);
315 buf->hdr = NULL;
316 return -1;
317 }
318
319 return 0;
320 }
321
322 int udebug_buf_init(struct udebug_buf *buf, size_t entries, size_t size)
323 {
324 uint32_t pagesz = sysconf(_SC_PAGESIZE);
325 char filename[] = "/udebug.XXXXXX";
326 unsigned int order = 12;
327 uint8_t ring_order = 5;
328 size_t head_size;
329 int fd;
330
331 INIT_LIST_HEAD(&buf->list);
332 if (size < pagesz)
333 size = pagesz;
334 while(size > 1 << order)
335 order++;
336 size = 1 << order;
337 while (entries > 1 << ring_order)
338 ring_order++;
339 entries = 1 << ring_order;
340
341 if (size > (1U << 29) || entries > (1U << 24))
342 return -1;
343
344 head_size = __udebug_headsize(entries, pagesz);
345 while (ALIGN(sizeof(*buf->hdr) + (entries * 2) * sizeof(struct udebug_ptr), pagesz) == head_size)
346 entries *= 2;
347
348 fd = shm_open_anon(filename);
349 if (fd < 0)
350 return -1;
351
352 if (ftruncate(fd, head_size + size) < 0)
353 goto err_close;
354
355 buf->head_size = head_size;
356 buf->data_size = size;
357 buf->ring_size = entries;
358 buf->fd = fd;
359
360 if (__udebug_buf_map(buf))
361 goto err_close;
362
363 buf->hdr->ring_size = entries;
364 buf->hdr->data_size = size;
365
366 /* ensure hdr changes are visible */
367 __sync_synchronize();
368
369 return 0;
370
371 err_close:
372 close(fd);
373 return -1;
374 }
375
376 static void *udebug_buf_alloc(struct udebug_buf *buf, uint32_t ofs, uint32_t len)
377 {
378 struct udebug_hdr *hdr = buf->hdr;
379
380 hdr->data_used = u32_max(hdr->data_used, ofs + len + 1);
381
382 /* ensure that data_used update is visible before clobbering data */
383 __sync_synchronize();
384
385 return udebug_buf_ptr(buf, ofs);
386 }
387
388 uint64_t udebug_buf_flags(struct udebug_buf *buf)
389 {
390 struct udebug_hdr *hdr = buf->hdr;
391 uint64_t flags;
392
393 if (!hdr)
394 return 0;
395
396 flags = hdr->flags[0];
397 if (sizeof(flags) != sizeof(uintptr_t))
398 flags |= ((uint64_t)hdr->flags[1]) << 32;
399
400 return flags;
401 }
402
403 void udebug_entry_init_ts(struct udebug_buf *buf, uint64_t timestamp)
404 {
405 struct udebug_hdr *hdr = buf->hdr;
406 struct udebug_ptr *ptr;
407
408 if (!hdr)
409 return;
410
411 ptr = udebug_ring_ptr(hdr, hdr->head);
412 ptr->start = hdr->data_head;
413 ptr->len = 0;
414 ptr->timestamp = timestamp;
415 }
416
417 void *udebug_entry_append(struct udebug_buf *buf, const void *data, uint32_t len)
418 {
419 struct udebug_hdr *hdr = buf->hdr;
420 struct udebug_ptr *ptr;
421 uint32_t ofs;
422 void *ret;
423
424 if (!hdr)
425 return NULL;
426
427 ptr = udebug_ring_ptr(hdr, hdr->head);
428 ofs = ptr->start + ptr->len;
429 if (ptr->len + len > buf->data_size / 2)
430 return NULL;
431
432 ret = udebug_buf_alloc(buf, ofs, len);
433 if (data)
434 memcpy(ret, data, len);
435 ptr->len += len;
436
437 return ret;
438 }
439
440 int udebug_entry_printf(struct udebug_buf *buf, const char *fmt, ...)
441 {
442 va_list ap;
443 size_t ret;
444
445 va_start(ap, fmt);
446 ret = udebug_entry_vprintf(buf, fmt, ap);
447 va_end(ap);
448
449 return ret;
450 }
451
452 int udebug_entry_vprintf(struct udebug_buf *buf, const char *fmt, va_list ap)
453 {
454 struct udebug_hdr *hdr = buf->hdr;
455 struct udebug_ptr *ptr;
456 uint32_t ofs;
457 uint32_t len;
458 char *str;
459
460 if (!hdr)
461 return -1;
462
463 ptr = udebug_ring_ptr(hdr, hdr->head);
464 ofs = ptr->start + ptr->len;
465 if (ptr->len > buf->data_size / 2)
466 return -1;
467
468 str = udebug_buf_alloc(buf, ofs, UDEBUG_MIN_ALLOC_LEN);
469 len = vsnprintf(str, UDEBUG_MIN_ALLOC_LEN, fmt, ap);
470 if (len <= UDEBUG_MIN_ALLOC_LEN)
471 goto out;
472
473 if (ptr->len + len > buf->data_size / 2)
474 return -1;
475
476 udebug_buf_alloc(buf, ofs, len + 1);
477 len = vsnprintf(str, len, fmt, ap);
478
479 out:
480 ptr->len += len;
481 return 0;
482 }
483
484 void udebug_entry_add(struct udebug_buf *buf)
485 {
486 struct udebug_hdr *hdr = buf->hdr;
487 struct udebug_ptr *ptr = udebug_ring_ptr(hdr, hdr->head);
488 uint32_t notify;
489 uint8_t *data;
490
491 /* ensure strings are always 0-terminated */
492 data = udebug_buf_ptr(buf, ptr->start + ptr->len);
493 *data = 0;
494 hdr->data_head = ptr->start + ptr->len + 1;
495
496 /* ensure that all data changes are visible before advancing head */
497 __sync_synchronize();
498
499 u32_set(&hdr->head, u32_get(&hdr->head) + 1);
500 if (!u32_get(&hdr->head))
501 u32_set(&hdr->head_hi, u32_get(&hdr->head_hi) + 1);
502
503 /* ensure that head change is visible */
504 __sync_synchronize();
505
506 notify = __atomic_exchange_n(&hdr->notify, 0, __ATOMIC_RELAXED);
507 if (notify) {
508 struct udebug_client_msg msg = {
509 .type = CL_MSG_RING_NOTIFY,
510 .id = buf->id,
511 .notify_mask = notify,
512 };
513 blob_buf_init(&b, 0);
514
515 udebug_send_msg(buf->ctx, &msg, b.head, -1);
516 }
517 }
518 void udebug_buf_free(struct udebug_buf *buf)
519 {
520 struct udebug *ctx = buf->ctx;
521
522 if (!list_empty(&buf->list) && buf->list.prev)
523 list_del(&buf->list);
524
525 if (ctx && ctx->fd.fd >= 0)
526 udebug_buf_msg(buf, CL_MSG_RING_REMOVE);
527
528 munmap(buf->hdr, buf->head_size + 2 * buf->data_size);
529 close(buf->fd);
530 memset(buf, 0, sizeof(*buf));
531 }
532
533 static void
534 __udebug_buf_add(struct udebug *ctx, struct udebug_buf *buf)
535 {
536 struct udebug_client_msg msg = {
537 .type = CL_MSG_RING_ADD,
538 .id = buf->id,
539 .ring_size = buf->hdr->ring_size,
540 .data_size = buf->hdr->data_size,
541 };
542 const struct udebug_buf_meta *meta = buf->meta;
543 void *c;
544
545 blob_buf_init(&b, 0);
546 blobmsg_add_string(&b, "name", meta->name);
547 c = blobmsg_open_array(&b, "flags");
548 for (size_t i = 0; i < meta->n_flags; i++) {
549 const struct udebug_buf_flag *flag = &meta->flags[i];
550 void *e = blobmsg_open_array(&b, NULL);
551 blobmsg_add_string(&b, NULL, flag->name);
552 blobmsg_add_u64(&b, NULL, flag->mask);
553 blobmsg_close_array(&b, e);
554 }
555 blobmsg_close_array(&b, c);
556
557 udebug_send_msg(ctx, &msg, b.head, buf->fd);
558 }
559
560 int udebug_buf_add(struct udebug *ctx, struct udebug_buf *buf,
561 const struct udebug_buf_meta *meta)
562 {
563 list_add_tail(&buf->list, &ctx->local_rings);
564 buf->ctx = ctx;
565 buf->meta = meta;
566 buf->id = ctx->next_id++;
567 buf->hdr->format = meta->format;
568 buf->hdr->sub_format = meta->sub_format;
569
570 if (ctx->fd.fd >= 0)
571 __udebug_buf_add(ctx, buf);
572
573 return 0;
574 }
575
576 void udebug_init(struct udebug *ctx)
577 {
578 INIT_LIST_HEAD(&ctx->local_rings);
579 avl_init(&ctx->remote_rings, udebug_id_cmp, true, NULL);
580 ctx->fd.fd = -1;
581 ctx->poll_handle = -1;
582 }
583
584 static void udebug_reconnect_cb(struct uloop_timeout *t)
585 {
586 struct udebug *ctx = container_of(t, struct udebug, reconnect);
587
588 if (udebug_connect(ctx, ctx->socket_path) < 0) {
589 uloop_timeout_set(&ctx->reconnect, 1000);
590 return;
591 }
592
593 udebug_add_uloop(ctx);
594 }
595
596 void udebug_auto_connect(struct udebug *ctx, const char *path)
597 {
598 free(ctx->socket_path);
599 ctx->reconnect.cb = udebug_reconnect_cb;
600 ctx->socket_path = path ? strdup(path) : NULL;
601 if (ctx->fd.fd >= 0)
602 return;
603
604 udebug_reconnect_cb(&ctx->reconnect);
605 }
606
607 int udebug_connect(struct udebug *ctx, const char *path)
608 {
609 struct udebug_remote_buf *rb;
610 struct udebug_buf *buf;
611
612 if (ctx->fd.fd >= 0)
613 close(ctx->fd.fd);
614 ctx->fd.fd = -1;
615
616 if (!path)
617 path = UDEBUG_SOCK_NAME;
618
619 ctx->fd.fd = usock(USOCK_UNIX, path, NULL);
620 if (ctx->fd.fd < 0)
621 return -1;
622
623 list_for_each_entry(buf, &ctx->local_rings, list)
624 __udebug_buf_add(ctx, buf);
625
626 avl_for_each_element(&ctx->remote_rings, rb, node) {
627 if (!rb->poll)
628 continue;
629
630 rb->poll = false;
631 udebug_remote_buf_set_poll(ctx, rb, true);
632 }
633
634 return 0;
635 }
636
637 static bool
638 udebug_recv_msg(struct udebug *ctx, struct udebug_client_msg *msg, int *fd,
639 bool wait)
640 {
641 struct iovec iov = {
642 .iov_base = msg,
643 .iov_len = sizeof(*msg)
644 };
645 int ret;
646
647 ret = recv_retry(ctx->fd.fd, &iov, wait, fd);
648 if (ret == -2)
649 uloop_fd_delete(&ctx->fd);
650
651 return ret == sizeof(*msg);
652 }
653
654 struct udebug_client_msg *__udebug_poll(struct udebug *ctx, int *fd, bool wait)
655 {
656 static struct udebug_client_msg msg = {};
657
658 while (udebug_recv_msg(ctx, &msg, fd, wait)) {
659 struct udebug_remote_buf *rb;
660 void *key;
661
662 if (msg.type != CL_MSG_RING_NOTIFY)
663 return &msg;
664
665 if (fd && *fd >= 0)
666 close(*fd);
667
668 if (!ctx->notify_cb)
669 continue;
670
671 key = (void *)(uintptr_t)msg.id;
672 rb = avl_find_element(&ctx->remote_rings, key, rb, node);
673 if (!rb || !rb->poll)
674 continue;
675
676 if (ctx->poll_handle >= 0)
677 __atomic_fetch_or(&rb->buf.hdr->notify,
678 1UL << ctx->poll_handle,
679 __ATOMIC_RELAXED);
680 ctx->notify_cb(ctx, rb);
681 }
682
683 return NULL;
684 }
685
686 void udebug_poll(struct udebug *ctx)
687 {
688 while (__udebug_poll(ctx, NULL, false));
689 }
690
691 static void udebug_fd_cb(struct uloop_fd *fd, unsigned int events)
692 {
693 struct udebug *ctx = container_of(fd, struct udebug, fd);
694
695 if (fd->eof)
696 uloop_fd_delete(fd);
697
698 udebug_poll(ctx);
699 }
700
701 void udebug_add_uloop(struct udebug *ctx)
702 {
703 if (ctx->fd.registered)
704 return;
705
706 ctx->fd.cb = udebug_fd_cb;
707 uloop_fd_add(&ctx->fd, ULOOP_READ);
708 }
709
710 void __udebug_disconnect(struct udebug *ctx, bool reconnect)
711 {
712 uloop_fd_delete(&ctx->fd);
713 close(ctx->fd.fd);
714 ctx->fd.fd = -1;
715 ctx->poll_handle = -1;
716 if (ctx->reconnect.cb && reconnect)
717 uloop_timeout_set(&ctx->reconnect, 1);
718 }
719
720 void udebug_free(struct udebug *ctx)
721 {
722 struct udebug_remote_buf *rb, *tmp;
723 struct udebug_buf *buf;
724
725 free(ctx->socket_path);
726 ctx->socket_path = NULL;
727
728 __udebug_disconnect(ctx, false);
729 uloop_timeout_cancel(&ctx->reconnect);
730
731 while (!list_empty(&ctx->local_rings)) {
732 buf = list_first_entry(&ctx->local_rings, struct udebug_buf, list);
733 udebug_buf_free(buf);
734 }
735
736 avl_for_each_element_safe(&ctx->remote_rings, rb, node, tmp)
737 udebug_remote_buf_unmap(ctx, rb);
738 }