4 #include <sys/socket.h>
14 #include <libubox/usock.h>
16 #define ALIGN(i, sz) (((i) + (sz) - 1) & ~((sz) - 1))
19 #define MAP_ANONYMOUS MAP_ANON
22 #define UDEBUG_MIN_ALLOC_LEN 128
23 static struct blob_buf b
;
25 static void __randname(char *template)
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;
37 int udebug_id_cmp(const void *k1
, const void *k2
, void *ptr
)
39 uint32_t id1
= (uint32_t)(uintptr_t)k1
, id2
= (uint32_t)(uintptr_t)k2
;
44 shm_open_anon(char *name
)
46 char *template = name
+ strlen(name
) - 6;
49 if (template < name
|| memcmp(template, "XXXXXX", 6) != 0)
52 for (int i
= 0; i
< 100; i
++) {
54 fd
= shm_open(name
, O_RDWR
| O_CREAT
| O_EXCL
, 0600);
56 if (shm_unlink(name
) < 0) {
63 if (fd
< 0 && errno
!= EEXIST
)
70 uint64_t udebug_timestamp(void)
75 clock_gettime(CLOCK_REALTIME
, &ts
);
79 val
+= ts
.tv_nsec
/ 1000;
85 __udebug_buf_map(struct udebug_buf
*buf
)
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
)
94 ptr2
= mmap(ptr
, buf
->head_size
+ buf
->data_size
,
95 PROT_READ
| PROT_WRITE
, MAP_FIXED
| MAP_SHARED
, buf
->fd
, 0);
99 ptr2
= mmap(ptr
+ buf
->head_size
+ buf
->data_size
, buf
->data_size
,
100 PROT_READ
| PROT_WRITE
, MAP_FIXED
| MAP_SHARED
, buf
->fd
,
102 if (ptr2
!= ptr
+ buf
->head_size
+ buf
->data_size
)
106 buf
->data
= ptr
+ buf
->head_size
;
110 munmap(ptr
, buf
->head_size
+ 2 * buf
->data_size
);
115 writev_retry(int fd
, struct iovec
*iov
, int iov_len
, int sock_fd
)
117 uint8_t fd_buf
[CMSG_SPACE(sizeof(int))] = { 0 };
118 struct msghdr msghdr
= { 0 };
119 struct cmsghdr
*cmsg
;
123 msghdr
.msg_iov
= iov
,
124 msghdr
.msg_iovlen
= iov_len
,
125 msghdr
.msg_control
= fd_buf
;
126 msghdr
.msg_controllen
= sizeof(fd_buf
);
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));
133 pfd
= (int *) CMSG_DATA(cmsg
);
134 msghdr
.msg_controllen
= cmsg
->cmsg_len
;
140 msghdr
.msg_control
= NULL
;
141 msghdr
.msg_controllen
= 0;
146 cur_len
= sendmsg(fd
, &msghdr
, 0);
148 struct pollfd pfd
= {
169 while (cur_len
>= (ssize_t
) iov
->iov_len
) {
170 cur_len
-= iov
->iov_len
;
176 iov
->iov_base
+= cur_len
;
177 iov
->iov_len
-= cur_len
;
178 msghdr
.msg_iov
= iov
;
179 msghdr
.msg_iovlen
= iov_len
;
182 /* Should never reach here */
187 recv_retry(int fd
, struct iovec
*iov
, bool wait
, int *recv_fd
)
189 uint8_t fd_buf
[CMSG_SPACE(sizeof(int))] = { 0 };
190 struct msghdr msghdr
= { 0 };
191 struct cmsghdr
*cmsg
;
196 msghdr
.msg_iov
= iov
,
197 msghdr
.msg_iovlen
= 1,
198 msghdr
.msg_control
= fd_buf
;
199 msghdr
.msg_controllen
= sizeof(fd_buf
);
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));
206 pfd
= (int *) CMSG_DATA(cmsg
);
208 while (iov
->iov_len
> 0) {
210 msghdr
.msg_control
= fd_buf
;
211 msghdr
.msg_controllen
= cmsg
->cmsg_len
;
213 msghdr
.msg_control
= NULL
;
214 msghdr
.msg_controllen
= 0;
218 bytes
= recvmsg(fd
, &msghdr
, 0);
241 iov
->iov_len
-= bytes
;
242 iov
->iov_base
+= bytes
;
245 if (iov
->iov_len
> 0) {
246 struct pollfd pfd
= {
252 ret
= poll(&pfd
, 1, UDEBUG_TIMEOUT
);
253 } while (ret
< 0 && errno
== EINTR
);
255 if (!(pfd
.revents
& POLLIN
))
263 void udebug_send_msg(struct udebug
*ctx
, struct udebug_client_msg
*msg
,
264 struct blob_attr
*meta
, int fd
)
266 struct iovec iov
[2] = {
267 { .iov_base
= msg
, .iov_len
= sizeof(*msg
) },
272 blob_buf_init(&b
, 0);
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
);
282 udebug_buf_msg(struct udebug_buf
*buf
, enum udebug_client_msg_type type
)
284 struct udebug_client_msg msg
= {
289 udebug_send_msg(buf
->ctx
, &msg
, NULL
, -1);
292 static size_t __udebug_headsize(unsigned int ring_size
, unsigned int page_size
)
294 ring_size
*= sizeof(struct udebug_ptr
);
295 return ALIGN(sizeof(struct udebug_hdr
) + ring_size
, page_size
);
298 int udebug_buf_open(struct udebug_buf
*buf
, int fd
, uint32_t ring_size
, uint32_t data_size
)
300 INIT_LIST_HEAD(&buf
->list
);
302 buf
->ring_size
= ring_size
;
303 buf
->head_size
= __udebug_headsize(ring_size
, sysconf(_SC_PAGESIZE
));
304 buf
->data_size
= data_size
;
306 if (buf
->ring_size
> (1U << 24) || buf
->data_size
> (1U << 29))
309 if (__udebug_buf_map(buf
))
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
);
322 int udebug_buf_init(struct udebug_buf
*buf
, size_t entries
, size_t size
)
324 uint32_t pagesz
= sysconf(_SC_PAGESIZE
);
325 char filename
[] = "/udebug.XXXXXX";
326 unsigned int order
= 12;
327 uint8_t ring_order
= 5;
331 INIT_LIST_HEAD(&buf
->list
);
334 while(size
> 1 << order
)
337 while (entries
> 1 << ring_order
)
339 entries
= 1 << ring_order
;
341 if (size
> (1U << 29) || entries
> (1U << 24))
344 head_size
= __udebug_headsize(entries
, pagesz
);
345 while (ALIGN(sizeof(*buf
->hdr
) + (entries
* 2) * sizeof(struct udebug_ptr
), pagesz
) == head_size
)
348 fd
= shm_open_anon(filename
);
352 if (ftruncate(fd
, head_size
+ size
) < 0)
355 buf
->head_size
= head_size
;
356 buf
->data_size
= size
;
357 buf
->ring_size
= entries
;
360 if (__udebug_buf_map(buf
))
363 buf
->hdr
->ring_size
= entries
;
364 buf
->hdr
->data_size
= size
;
366 /* ensure hdr changes are visible */
367 __sync_synchronize();
376 static void *udebug_buf_alloc(struct udebug_buf
*buf
, uint32_t ofs
, uint32_t len
)
378 struct udebug_hdr
*hdr
= buf
->hdr
;
380 hdr
->data_used
= u32_max(hdr
->data_used
, ofs
+ len
+ 1);
382 /* ensure that data_used update is visible before clobbering data */
383 __sync_synchronize();
385 return udebug_buf_ptr(buf
, ofs
);
388 uint64_t udebug_buf_flags(struct udebug_buf
*buf
)
390 struct udebug_hdr
*hdr
= buf
->hdr
;
396 flags
= hdr
->flags
[0];
397 if (sizeof(flags
) != sizeof(uintptr_t))
398 flags
|= ((uint64_t)hdr
->flags
[1]) << 32;
403 void udebug_entry_init_ts(struct udebug_buf
*buf
, uint64_t timestamp
)
405 struct udebug_hdr
*hdr
= buf
->hdr
;
406 struct udebug_ptr
*ptr
;
411 ptr
= udebug_ring_ptr(hdr
, hdr
->head
);
412 ptr
->start
= hdr
->data_head
;
414 ptr
->timestamp
= timestamp
;
417 void *udebug_entry_append(struct udebug_buf
*buf
, const void *data
, uint32_t len
)
419 struct udebug_hdr
*hdr
= buf
->hdr
;
420 struct udebug_ptr
*ptr
;
427 ptr
= udebug_ring_ptr(hdr
, hdr
->head
);
428 ofs
= ptr
->start
+ ptr
->len
;
429 if (ptr
->len
+ len
> buf
->data_size
/ 2)
432 ret
= udebug_buf_alloc(buf
, ofs
, len
);
434 memcpy(ret
, data
, len
);
440 int udebug_entry_printf(struct udebug_buf
*buf
, const char *fmt
, ...)
446 ret
= udebug_entry_vprintf(buf
, fmt
, ap
);
452 int udebug_entry_vprintf(struct udebug_buf
*buf
, const char *fmt
, va_list ap
)
454 struct udebug_hdr
*hdr
= buf
->hdr
;
455 struct udebug_ptr
*ptr
;
463 ptr
= udebug_ring_ptr(hdr
, hdr
->head
);
464 ofs
= ptr
->start
+ ptr
->len
;
465 if (ptr
->len
> buf
->data_size
/ 2)
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
)
473 if (ptr
->len
+ len
> buf
->data_size
/ 2)
476 udebug_buf_alloc(buf
, ofs
, len
+ 1);
477 len
= vsnprintf(str
, len
, fmt
, ap
);
484 void udebug_entry_add(struct udebug_buf
*buf
)
486 struct udebug_hdr
*hdr
= buf
->hdr
;
487 struct udebug_ptr
*ptr
= udebug_ring_ptr(hdr
, hdr
->head
);
491 /* ensure strings are always 0-terminated */
492 data
= udebug_buf_ptr(buf
, ptr
->start
+ ptr
->len
);
494 hdr
->data_head
= ptr
->start
+ ptr
->len
+ 1;
496 /* ensure that all data changes are visible before advancing head */
497 __sync_synchronize();
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);
503 /* ensure that head change is visible */
504 __sync_synchronize();
506 notify
= __atomic_exchange_n(&hdr
->notify
, 0, __ATOMIC_RELAXED
);
508 struct udebug_client_msg msg
= {
509 .type
= CL_MSG_RING_NOTIFY
,
511 .notify_mask
= notify
,
513 blob_buf_init(&b
, 0);
515 udebug_send_msg(buf
->ctx
, &msg
, b
.head
, -1);
518 void udebug_buf_free(struct udebug_buf
*buf
)
520 struct udebug
*ctx
= buf
->ctx
;
522 if (!list_empty(&buf
->list
) && buf
->list
.prev
)
523 list_del(&buf
->list
);
525 if (ctx
&& ctx
->fd
.fd
>= 0)
526 udebug_buf_msg(buf
, CL_MSG_RING_REMOVE
);
528 munmap(buf
->hdr
, buf
->head_size
+ buf
->data_size
);
530 memset(buf
, 0, sizeof(*buf
));
534 __udebug_buf_add(struct udebug
*ctx
, struct udebug_buf
*buf
)
536 struct udebug_client_msg msg
= {
537 .type
= CL_MSG_RING_ADD
,
539 .ring_size
= buf
->hdr
->ring_size
,
540 .data_size
= buf
->hdr
->data_size
,
542 const struct udebug_buf_meta
*meta
= buf
->meta
;
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
);
555 blobmsg_close_array(&b
, c
);
557 udebug_send_msg(ctx
, &msg
, b
.head
, buf
->fd
);
560 int udebug_buf_add(struct udebug
*ctx
, struct udebug_buf
*buf
,
561 const struct udebug_buf_meta
*meta
)
563 list_add_tail(&buf
->list
, &ctx
->local_rings
);
566 buf
->id
= ctx
->next_id
++;
567 buf
->hdr
->format
= meta
->format
;
568 buf
->hdr
->sub_format
= meta
->sub_format
;
571 __udebug_buf_add(ctx
, buf
);
576 void udebug_init(struct udebug
*ctx
)
578 INIT_LIST_HEAD(&ctx
->local_rings
);
579 avl_init(&ctx
->remote_rings
, udebug_id_cmp
, true, NULL
);
581 ctx
->poll_handle
= -1;
584 static void udebug_reconnect_cb(struct uloop_timeout
*t
)
586 struct udebug
*ctx
= container_of(t
, struct udebug
, reconnect
);
588 if (udebug_connect(ctx
, ctx
->socket_path
) < 0) {
589 uloop_timeout_set(&ctx
->reconnect
, 1000);
593 udebug_add_uloop(ctx
);
596 void udebug_auto_connect(struct udebug
*ctx
, const char *path
)
598 free(ctx
->socket_path
);
599 ctx
->reconnect
.cb
= udebug_reconnect_cb
;
600 ctx
->socket_path
= path
? strdup(path
) : NULL
;
604 udebug_reconnect_cb(&ctx
->reconnect
);
607 int udebug_connect(struct udebug
*ctx
, const char *path
)
609 struct udebug_remote_buf
*rb
;
610 struct udebug_buf
*buf
;
617 path
= UDEBUG_SOCK_NAME
;
619 ctx
->fd
.fd
= usock(USOCK_UNIX
, path
, NULL
);
623 list_for_each_entry(buf
, &ctx
->local_rings
, list
)
624 __udebug_buf_add(ctx
, buf
);
626 avl_for_each_element(&ctx
->remote_rings
, rb
, node
) {
631 udebug_remote_buf_set_poll(ctx
, rb
, true);
638 udebug_recv_msg(struct udebug
*ctx
, struct udebug_client_msg
*msg
, int *fd
,
643 .iov_len
= sizeof(*msg
)
647 ret
= recv_retry(ctx
->fd
.fd
, &iov
, wait
, fd
);
649 uloop_fd_delete(&ctx
->fd
);
651 return ret
== sizeof(*msg
);
654 struct udebug_client_msg
*__udebug_poll(struct udebug
*ctx
, int *fd
, bool wait
)
656 static struct udebug_client_msg msg
= {};
658 while (udebug_recv_msg(ctx
, &msg
, fd
, wait
)) {
659 struct udebug_remote_buf
*rb
;
662 if (msg
.type
!= CL_MSG_RING_NOTIFY
)
671 key
= (void *)(uintptr_t)msg
.id
;
672 rb
= avl_find_element(&ctx
->remote_rings
, key
, rb
, node
);
673 if (!rb
|| !rb
->poll
)
676 if (ctx
->poll_handle
>= 0)
677 __atomic_fetch_or(&rb
->buf
.hdr
->notify
,
678 1UL << ctx
->poll_handle
,
680 ctx
->notify_cb(ctx
, rb
);
686 void udebug_poll(struct udebug
*ctx
)
688 while (__udebug_poll(ctx
, NULL
, false));
691 static void udebug_fd_cb(struct uloop_fd
*fd
, unsigned int events
)
693 struct udebug
*ctx
= container_of(fd
, struct udebug
, fd
);
701 void udebug_add_uloop(struct udebug
*ctx
)
703 if (ctx
->fd
.registered
)
706 ctx
->fd
.cb
= udebug_fd_cb
;
707 uloop_fd_add(&ctx
->fd
, ULOOP_READ
);
710 void __udebug_disconnect(struct udebug
*ctx
, bool reconnect
)
712 uloop_fd_delete(&ctx
->fd
);
715 ctx
->poll_handle
= -1;
716 if (ctx
->reconnect
.cb
&& reconnect
)
717 uloop_timeout_set(&ctx
->reconnect
, 1);
720 void udebug_free(struct udebug
*ctx
)
722 struct udebug_remote_buf
*rb
, *tmp
;
723 struct udebug_buf
*buf
;
725 free(ctx
->socket_path
);
726 ctx
->socket_path
= NULL
;
728 __udebug_disconnect(ctx
, false);
729 uloop_timeout_cancel(&ctx
->reconnect
);
731 while (!list_empty(&ctx
->local_rings
)) {
732 buf
= list_first_entry(&ctx
->local_rings
, struct udebug_buf
, list
);
733 udebug_buf_free(buf
);
736 avl_for_each_element_safe(&ctx
->remote_rings
, rb
, node
, tmp
)
737 udebug_remote_buf_unmap(ctx
, rb
);