From cbea4ad7a87fabeaba0705b7e1a72b08418bc36f Mon Sep 17 00:00:00 2001
From: XANTRONIX Development <dev@xantronix.com>
Date: Thu, 2 Jul 2020 23:54:48 -0400
Subject: [PATCH] Implement src/print.c

Implement src/print.c to provide facilities for pretty printing packets
to any FILE handle
---
 include/patty/print.h |  11 ++++
 src/Makefile          |   4 +-
 src/print.c           | 127 ++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 140 insertions(+), 2 deletions(-)
 create mode 100644 include/patty/print.h
 create mode 100644 src/print.c

diff --git a/include/patty/print.h b/include/patty/print.h
new file mode 100644
index 0000000..e018b63
--- /dev/null
+++ b/include/patty/print.h
@@ -0,0 +1,11 @@
+#ifndef _PATTY_PRINT_H
+#define _PATTY_PRINT_H
+
+int patty_print_hexdump(FILE *fh, void *data, size_t len);
+
+int patty_print_frame(FILE *fh,
+                      const patty_ax25_frame *frame,
+                      void *buf,
+                      size_t len);
+
+#endif /* _PATTY_PRINT_H */
diff --git a/src/Makefile b/src/Makefile
index 9c51b55..8480580 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -9,10 +9,10 @@ LDFLAGS		=
 
 HEADERS		= kiss.h ax25.h ax25/if.h ax25/macros.h ax25/proto.h \
 		  ax25/call.h ax25/frame.h ax25/sock.h ax25/route.h \
-		  ax25/server.h list.h hash.h dict.h
+		  ax25/server.h list.h hash.h dict.h print.h
 
 OBJS		= kiss.o ax25.o if.o call.o frame.o sock.o route.o server.o \
-		  list.o hash.o dict.o
+		  list.o hash.o dict.o print.o
 
 EXAMPLES	= testserver testclient-connect testclient-listen decode
 
diff --git a/src/print.c b/src/print.c
new file mode 100644
index 0000000..a0ab8f9
--- /dev/null
+++ b/src/print.c
@@ -0,0 +1,127 @@
+#include <stdio.h>
+#include <inttypes.h>
+
+#include <patty/ax25.h>
+#include <patty/print.h>
+
+#define printable(c) \
+    (c >= 0x20 && c < 0x7f)
+
+int patty_print_hexdump(FILE *fh, void *data, size_t len) {
+    size_t i;
+
+    for (i=0; i<len; i+=16) {
+        size_t x;
+
+        if (fprintf(fh, "%08lx:", i) < 0) {
+            goto error_io;
+        }
+
+        for (x=0; x<16; x++) {
+            if (!(x & 1)) {
+                if (fprintf(fh, " ") < 0) {
+                    goto error_io;
+                }
+            }
+
+            if (i+x < len) {
+                if (fprintf(fh, "%02x", ((uint8_t *)data)[i+x]) < 0) {
+                    goto error_io;
+                }
+            } else {
+                if (fprintf(fh, "  ") < 0) {
+                    goto error_io;
+                }
+            }
+        }
+
+        if (fprintf(fh, "  ") < 0) {
+            goto error_io;
+        }
+
+        for (x=0; x<16 && i+x<len; x++) {
+            uint8_t c = ((uint8_t *)data)[i+x];
+
+            if (fputc(printable(c)? c: '.', fh) < 0) {
+                goto error_io;
+            }
+        }
+
+        if (fprintf(fh, "\n") < 0) {
+            goto error_io;
+        }
+    }
+
+    return 0;
+
+error_io:
+    return -1;
+}
+
+static int print_addr(FILE *fh, const patty_ax25_addr *addr) {
+    char buf[7];
+    uint8_t ssid;
+
+    if (patty_ax25_ntop(addr, buf, &ssid, sizeof(buf)) < 0) {
+        goto error_ntop;
+    }
+
+    fprintf(fh, "%s", buf);
+
+    if (ssid) {
+        fprintf(fh, "/%d", ssid);
+    }
+
+    return 0;
+
+error_ntop:
+    return -1;
+}
+
+static int print_frame_addrs(FILE *fh, const patty_ax25_frame *frame) {
+    int i;
+
+    print_addr(fh, &frame->src);
+
+    fprintf(fh, " > ");
+
+    for (i=0; i<frame->hops; i++) {
+        print_addr(fh, &frame->repeaters[i]);
+        fprintf(fh, " > ");
+    }
+
+    print_addr(fh, &frame->dest);
+
+    return 0;
+}
+
+int patty_print_frame(FILE *fh,
+                      const patty_ax25_frame *frame,
+                      void *buf,
+                      size_t len) {
+    if (print_frame_addrs(fh, frame) < 0) {
+        goto error_io;
+    }
+
+    if (PATTY_AX25_CONTROL_INFO(frame->control)) {
+        if (fprintf(fh, " type I poll %d ns %d nr %d info %zu bytes",
+                PATTY_AX25_CONTROL_POLL(frame->control),
+                PATTY_AX25_CONTROL_SEQ_SEND(frame->control),
+                PATTY_AX25_CONTROL_SEQ_RECV(frame->control),
+                frame->infolen) < 0) {
+            goto error_io;
+        }
+    } else if (PATTY_AX25_CONTROL_UNNUMBERED(frame->control)) {
+        if (fprintf(fh, " type U info %zu bytes",
+                frame->infolen) < 0) {
+            goto error_io;
+        }
+    }
+
+    fprintf(fh, "\n");
+
+    return patty_print_hexdump(fh, buf, len);
+
+error_io:
+    return -1;
+}