working uprobes

src/ftracetool.c

@@ -120,14 +120,14 @@ unsigned long lookup_symbol_in_file(const char *const filename,
     if (fd == -1)
     {
         perror("open() failed");
-        return EXIT_FAILURE;
+        return 0;
     }
     (void)elf_version(EV_CURRENT);
 
     if ((elf = elf_begin(fd, ELF_C_READ, NULL)) == NULL)
     {
         fprintf(stderr, "elf_begin() failed: %s\n", elf_errmsg(elf_errno()));
-        return EXIT_FAILURE;
+        return 0;
     }
 
     while ((scn = elf_nextscn(elf, scn)) != NULL)
@@ -190,7 +190,7 @@ unsigned long lookup_symbol_in_file(const char *const filename,
     }
     elf_end(elf);
 
-    return -1;
+    return 0;
 }
 
 static int do_show_symbols_in_file(const char *const filename)
@@ -289,7 +289,211 @@ static sexp sexp_lookup_symbol_in_file_stub(
     return res;
 }
 
-static int register_ftracetool_api(sexp ctx)
+static sexp display_exception(sexp ctx, sexp self, sexp_sint_t n, sexp arg0)
+{
+    sexp res;
+    if (!sexp_exceptionp(arg0))
+        return sexp_type_exception(ctx, self, SEXP_EXCEPTION, arg0);
+    sexp_print_exception(ctx, arg0, sexp_current_error_port(ctx));
+    return res;
+}
+
+static sexp sexp_sleep_stub(sexp ctx, sexp self, sexp_sint_t n, sexp arg0)
+{
+    sexp res;
+    if (!sexp_numberp(arg0))
+        return sexp_type_exception(ctx, self, SEXP_NUMBER, arg0);
+    sleep(sexp_uint_value(arg0));
+    return res;
+}
+
+static int parse_ftrace_entry(const char *str,
+                              char *comm_buf,
+                              char *pid_buf,
+                              char *timestamp_buf,
+                              char *probe_id_buf,
+                              char *args_buf)
+{
+    // skip whitespaces
+    while (isspace(*str))
+    {
+        str++;
+    }
+    if (!*str)
+    {
+        return -1;
+    }
+
+    char *out = comm_buf;
+    while (*str != '-')
+    {
+        if (!*str)
+        {
+            return -2;
+        }
+        // TODO check out buf overflow
+        *out++ = *str++;
+    }
+    if (!*str)
+    {
+        return -3;
+    }
+    *out = 0;
+    str++; // Skip '-
+
+    out = pid_buf;
+    while (isdigit(*str))
+    {
+        if (!*str)
+        {
+            return -4;
+        }
+
+        // TODO check out buf overflow
+        *out++ = *str++;
+    }
+    *out = 0;
+
+    // skip everything until ]
+    while (*str != ']')
+    {
+        str++;
+    }
+    if (!*str)
+    {
+        return -5;
+    }
+    str++;
+    if (!*str)
+    {
+        return -6;
+    }
+
+    // skip whitespaces && flags
+    while (isspace(*str) || isalpha(*str))
+    {
+        str++;
+    }
+    if (!*str)
+    {
+        return -7;
+    }
+
+    out = timestamp_buf;
+    while (isdigit(*str) || *str == '.')
+    {
+        if (!*str)
+        {
+            return -8;
+        }
+
+        // TODO check out buf overflow
+        *out++ = *str++;
+    }
+    *out = 0;
+
+    if (*str != ':')
+    {
+        return -9;
+    }
+    str++;
+    // skip whitespaces
+    while (isspace(*str))
+    {
+        str++;
+    }
+    if (!*str)
+    {
+        return -10;
+    }
+
+    out = probe_id_buf;
+    while (isalnum(*str) || *str == '_')
+    {
+        if (!*str)
+        {
+            return -11;
+        }
+        // TODO check out buf overflow
+        *out++ = *str++;
+    }
+    *out = 0;
+
+    // skip everything until )
+    while (*str != ')')
+    {
+        str++;
+    }
+    if (!*str)
+    {
+        return -12;
+    }
+    str++;
+    if (!*str)
+    {
+        return -13;
+    }
+
+    out = args_buf;
+    while (*str)
+    {
+        if (!*str)
+        {
+            return -14;
+        }
+        // TODO check out buf overflow
+        *out++ = *str++;
+    }
+    *out = 0;
+
+    return 0;
+}
+
+static sexp
+sexp_parse_ftrace_entry_stub(sexp ctx, sexp self, sexp_sint_t n, sexp arg0)
+{
+    sexp res;
+    int ret = 0;
+    if (!sexp_stringp(arg0))
+    {
+        return sexp_type_exception(ctx, self, SEXP_STRING, arg0);
+    }
+    char comm_buf[128];
+    char pid_buf[32];
+    char timestamp_buf[64];
+    char probe_id_buf[64];
+    char args[128];
+    ret = parse_ftrace_entry(sexp_string_data(arg0),
+                             comm_buf,
+                             pid_buf,
+                             timestamp_buf,
+                             probe_id_buf,
+                             args);
+    if (ret)
+    {
+        return SEXP_FALSE;
+    }
+    unsigned long pid = atoi(pid_buf);
+    char *dot = strchr(timestamp_buf, '.');
+    *dot = 0;
+    char *frac = dot + 1;
+    double timestamp = atol(timestamp_buf) + atol(frac) / 1000000.;
+
+    return sexp_cons(
+        ctx,
+        sexp_c_string(ctx, comm_buf, -1),
+        sexp_cons(ctx,
+                  sexp_make_fixnum(pid),
+                  sexp_cons(ctx,
+                            sexp_make_flonum(ctx, timestamp),
+                            sexp_cons(ctx,
+                                      sexp_c_string(ctx, probe_id_buf, -1),
+                                      sexp_cons(ctx,
+                                                sexp_c_string(ctx, args, -1),
+                                                SEXP_NULL)))));
+}
+
+static int register_lookup_symbol_in_file(sexp ctx)
 {
     sexp_gc_var1(op);
     sexp_gc_preserve1(ctx, op);
@@ -309,6 +513,106 @@ static int register_ftracetool_api(sexp ctx)
     return 0;
 }
 
+static int register_display_exception(sexp ctx)
+{
+    sexp_gc_var1(op);
+    sexp_gc_preserve1(ctx, op);
+    op = sexp_define_foreign(
+        ctx, sexp_context_env(ctx), "display-exception", 1, display_exception);
+    if (sexp_opcodep(op))
+    {
+        sexp_opcode_return_type(op) = sexp_make_fixnum(SEXP_NULL);
+        sexp_opcode_arg1_type(op) = sexp_make_fixnum(SEXP_EXCEPTION);
+    }
+    sexp_gc_release1(ctx);
+
+    return 0;
+}
+
+static int register_sleep(sexp ctx)
+{
+    sexp_gc_var1(op);
+    sexp_gc_preserve1(ctx, op);
+    op = sexp_define_foreign(
+        ctx, sexp_context_env(ctx), "sleep", 1, sexp_sleep_stub);
+    if (sexp_opcodep(op))
+    {
+        sexp_opcode_return_type(op) = sexp_make_fixnum(SEXP_NULL);
+        sexp_opcode_arg1_type(op) = sexp_make_fixnum(SEXP_NUMBER);
+    }
+    sexp_gc_release1(ctx);
+
+    return 0;
+}
+
+sexp sexp_open_append_file_op(sexp ctx, sexp self, sexp_sint_t n, sexp path)
+{
+    FILE *out;
+    int count = 0;
+    sexp_assert_type(ctx, sexp_stringp, SEXP_STRING, path);
+    do
+    {
+        if (count != 0)
+            sexp_gc(ctx, NULL);
+        out = fopen(sexp_string_data(path), "a+");
+    } while (!out && sexp_out_of_file_descriptors() && !count++);
+    if (!out)
+        return sexp_file_exception(
+            ctx, self, "couldn't open output file", path);
+#if SEXP_USE_GREEN_THREADS
+    fcntl(fileno(out), F_SETFL, O_NONBLOCK);
+#endif
+    return sexp_make_output_port(ctx, out, path);
+}
+
+static int register_open_append_file(sexp ctx)
+{
+    sexp_gc_var1(op);
+    sexp_gc_preserve1(ctx, op);
+    op = sexp_define_foreign(ctx,
+                             sexp_context_env(ctx),
+                             "open-append-file",
+                             1,
+                             sexp_open_append_file_op);
+    if (sexp_opcodep(op))
+    {
+        sexp_opcode_return_type(op) = sexp_make_fixnum(SEXP_OPORT);
+        sexp_opcode_arg1_type(op) = sexp_make_fixnum(SEXP_NUMBER);
+    }
+    sexp_gc_release1(ctx);
+
+    return 0;
+}
+
+static int register_parse_ftrace_entry(sexp ctx)
+{
+    sexp_gc_var1(op);
+    sexp_gc_preserve1(ctx, op);
+    op = sexp_define_foreign(ctx,
+                             sexp_context_env(ctx),
+                             "parse-ftrace-entry",
+                             1,
+                             sexp_parse_ftrace_entry_stub);
+    if (sexp_opcodep(op))
+    {
+        sexp_opcode_return_type(op) = sexp_make_fixnum(SEXP_PAIR);
+        sexp_opcode_arg1_type(op) = sexp_make_fixnum(SEXP_STRING);
+    }
+    sexp_gc_release1(ctx);
+
+    return 0;
+}
+
+static int register_ftracetool_api(sexp ctx)
+{
+    register_lookup_symbol_in_file(ctx);
+    register_display_exception(ctx);
+    register_sleep(ctx);
+    register_open_append_file(ctx);
+    register_parse_ftrace_entry(ctx);
+    return 0;
+}
+
 static int eval_ftracetool_scm(sexp ctx)
 {
     sexp_gc_var2(ret, fname);

src/ftracetool.scm

@@ -1,5 +1,9 @@
 (import (scheme base))
+(import (srfi 1))
 (import (srfi 16))
+(import (srfi 115))
+(import (srfi 130))
+(import (chibi string))
 
 (define (mapping-str key)
   (let ((pair (assoc key *ftt-mappings*)))
@@ -12,6 +16,14 @@
     (when value
       (string->number value))))
 
+(define (with-append-to-file file thunk)
+  (let ((old-out (current-output-port))
+        (tmp-out (open-append-file file)))
+    (dynamic-wind
+      (lambda () (current-output-port tmp-out))
+      (lambda () (let ((res (thunk))) (close-output-port tmp-out) res))
+      (lambda () (current-output-port old-out)))))
+
 (define-record-type probe
   (make-probe
    install-handler
@@ -22,30 +34,241 @@
   (remove-handler probe-remove-handler)
   (process-handler probe-process-handler))
 
+(define-record-type ftrace-probe
+  (make-ftrace-probe
+   name
+   install-handler
+   remove-handler
+   process-entry-handler
+   subsystem)
+  ftrace-probe?
+  (name ftrace-probe-name)
+  (install-handler ftrace-probe-install-handler)
+  (remove-handler ftrace-probe-remove-handler)
+  (process-entry-handler ftrace-probe-process-entry-handler)
+  (subsystem ftrace-probe-subsystem))
+
+(define-record-type trace-event
+  (make-trace-event
+   timestamp
+   probe-id
+   pid
+   arguments)
+  trace-event?
+  (timestamp trace-event-timestamp)
+  (probe-id trace-event-probe-id)
+  (pid trace-event-pid)
+  (arguments trace-event-arguments))
+
 (define define-probes
   (case-lambda
-   (() (display "Do waiting\n"))
+   (()
+    (display "Do waiting\n")
+    (sleep 1))
    ((probe . rest)
-    (display "Got probe: ")
-    (display probe)
-    (newline)
     (let ((token ((probe-install-handler probe))))
       (guard (x
               (else
-               (display "Error:")
+               (display-exception x)
-               (display x)
                (newline)))
              (apply define-probes rest)
              ((probe-process-handler probe)))
-      ((probe-remove-handler probe) token))
+      ((probe-remove-handler probe) token)))))
-    ))
+
-  )
+(define (display-uprobe-install-string probe-name binary-path symbol-offset args)
+  (display "p:")
+  (display probe-name)
+  (display " ")
+  (display binary-path)
+  (display ":0x")
+  (display (number->string symbol-offset 16))
+  (display " ")
+  (for-each
+   (lambda (x)
+     (display x)
+     (display " ")) args)
+  (newline))
+
+(define (display-uretprobe-install-string probe-name binary-path symbol-offset args)
+  (display "r:")
+  (display probe-name)
+  (display " ")
+  (display binary-path)
+  (display ":0x")
+  (display (number->string symbol-offset 16))
+  (display " ")
+  (for-each
+   (lambda (x)
+     (display x)
+     (display " ")) args)
+  (newline))
+
+(define (ftrace-uprobe-install probe-name binary-path symbol args)
+  (let ((symbol-offset (lookup-symbol-in-file binary-path symbol)))
+    (if (eq? symbol-offset 0)
+        (raise "Unable to locate symbol in binary file")
+        (begin
+          (display "Installing uprobe to ")
+          (display binary-path)
+          (newline)
+          (display-uprobe-install-string probe-name binary-path symbol-offset args)
+          (with-append-to-file "/sys/kernel/tracing/uprobe_events"
+            (lambda ()
+              (display-uprobe-install-string probe-name binary-path symbol-offset args)))))))
+
+(define (ftrace-uprobe-remove probe-name)
+  (display "Removing probe: ")
+  (display probe-name)
+  (newline)
+  (with-output-to-file "/sys/kernel/tracing/uprobe_events"
+    (lambda ()
+      (display "-:")
+      (display probe-name)
+      (newline))))
 
 (define (ftrace-uprobe probe-name binary-path symbol args handler)
-  (list 'uprobe probe-name binary-path symbol args handler))
+  (make-ftrace-probe
+   probe-name
+   (lambda () (ftrace-uprobe-install probe-name binary-path symbol args))
+   (lambda () (ftrace-uprobe-remove probe-name))
+   (lambda (event) (handler event))
+   'uprobe))
+
+(define (ftrace-uretprobe-install probe-name binary-path symbol args)
+  (let ((symbol-offset (lookup-symbol-in-file binary-path symbol)))
+    (if (eq? symbol-offset 0)
+        (raise "Unable to locate symbol in binary file")
+        (begin
+          (display "Installing uretprobe to ")
+          (display binary-path)
+          (newline)
+          (display-uretprobe-install-string probe-name binary-path symbol-offset args)
+          (with-append-to-file "/sys/kernel/tracing/uprobe_events"
+            (lambda ()
+              (display-uretprobe-install-string probe-name binary-path symbol-offset args)))))))
+
+(define (ftrace-uretprobe-remove probe-name)
+  (display "Removing probe: ")
+  (display probe-name)
+  (newline)
+  (with-output-to-file "/sys/kernel/tracing/uprobe_events"
+    (lambda ()
+      (display "-:")
+      (display probe-name)
+      (newline))))
+
+(define (ftrace-uretprobe probe-name binary-path symbol args handler)
+  (make-ftrace-probe
+   probe-name
+   (lambda () (ftrace-uretprobe-install probe-name binary-path symbol args))
+   (lambda () (ftrace-uretprobe-remove probe-name))
+   (lambda (event) (handler event))
+   'uprobe))
+
+(define (ftrace . subprobes)
+  (make-probe
+   (lambda () (ftrace-install subprobes))
+   (lambda (_) (ftrace-remove subprobes))
+   (lambda () (ftrace-process subprobes))))
+
+(define (ftrace-install subprobes)
+  (let* ((all-systems (delete-duplicates (map ftrace-probe-subsystem subprobes))))
+    (for-each (lambda (subprobe)
+                ((ftrace-probe-install-handler subprobe))) subprobes)
+    ;; (for-each (lambda (x)
+    ;;             (cond
+    ;;              ((eqv? x ' uprobe)
+    ;;               (display "uprobe"))
+    ;;              (else
+    ;;               (display "unknown system: ")
+    ;;               (display x)
+    ;;               (newline)))) all-systems)
+  (with-output-to-file "/sys/kernel/tracing/events/uprobes/enable"
+    (lambda ()
+      (display "1")
+      (newline)))
+    ))
+
+(define (ftrace-remove subprobes)
+    (for-each (lambda (subprobe)
+                ((ftrace-probe-remove-handler subprobe))) subprobes))
+
+(define arg-rx
+  (regexp '(: (+ (~ "=")) (+ (~ " ")))))
+
+(define (parse-arguments args)
+  (let ((raw-args (regexp-extract arg-rx args)))
+    (map
+     (lambda (x)
+       (substring/cursors x
+                         (string-cursor-next x (string-index x #\=))
+                         (string-cursor-end x))) raw-args)))
+
+(define (read-trace-events-from-file filename)
+  (let ((rx (regexp '(:
+                      (+ whitespace)
+                      ($ alnum (+ (~ "-")))
+                      "-"
+                      ($ word)
+                      (+ whitespace)
+                      "["
+                      (+ num)
+                      "]"
+                      (+ whitespace)
+                      (+ (~ whitespace))
+                      (+ whitespace)
+                      ($ (+ (or num ".")))
+                      ":"
+                      (+ whitespace)
+                      ($ (+ (or alnum "_")))
+                      ": ("
+                      (+ (~ ")"))
+                      ")"
+                      ($ (+ any))
+                      ))))
+    (reverse
+     (call-with-input-file filename
+       (lambda (p)
+         (let loop ((line (read-line p))
+                    (events '())
+                    (n 0))
+           (if (or (eof-object? line)
+                   (> n 1000))
+               events
+               (begin
+                (let ((match (parse-ftrace-entry line)))
+                  (if match
+                    (let* ((comm (list-ref match 0))
+                           (pid (list-ref match 1))
+                           (timestamp (list-ref match 2))
+                           (probe-name (list-ref match 3))
+                           (args (list-ref match 4))
+                           (parsed-args (parse-arguments args))
+                           (trace-event (make-trace-event timestamp probe-name pid (parse-arguments args))))
+                      (loop (read-line p) (cons trace-event events) (+ 1 n)))
+                    (loop (read-line p) events n)))))))))))
+
+(define (find-subprobe probe-name subprobes)
+  (find (lambda (x) (equal? probe-name (ftrace-probe-name x))) subprobes))
+
+(define (ftrace-process subprobes)
+    (with-output-to-file "/sys/kernel/tracing/events/uprobes/enable"
+      (lambda ()
+        (display "0")
+        (newline)))
+    (let ((events (read-trace-events-from-file "/sys/kernel/tracing/trace")))
+      (for-each
+       (lambda (event)
+         (let ((probe (find-subprobe (trace-event-probe-id event) subprobes)))
+           (when probe
+             ((ftrace-probe-process-entry-handler probe) event)))
+         ) events)))
 
 (define (tracing-probe ctx install remove process)
   (make-probe (lambda () (install ctx))
               (lambda (_) (remove ctx))
               (lambda () (process ctx))))
 
+(define (trace-event-arg event num)
+  (list-ref (trace-event-arguments event) num))
+