A way of getting debug output via openocd

debug.c

#include <stdint.h>
#include <stddef.h>
#include <stdarg.h>
#include <stdio.h>

volatile uint8_t dbg_tx_tail = 0;
volatile uint8_t dbg_tx_head = 0;
volatile uint8_t dbg_tx_buf[128] = {0};

volatile uint8_t dbg_rx_tail = 0;
volatile uint8_t dbg_rx_head = 0;
volatile uint8_t dbg_rx_buf[128] = {0};

char line_buf[128] = {0};

void debug_putc(uint8_t x) {
    // Wait for space
    while(((dbg_tx_head - dbg_tx_tail)&0xFF) == 0x80) {}

    // Write
    dbg_tx_buf[dbg_tx_head & 0x7F] = x;
    dbg_tx_head++;
}

void debug_puthex(uint8_t x) {
    const char hex[] = "0123456789ABCDEF";
    debug_putc(hex[(x >> 4) & 0xF]);
    debug_putc(hex[(x >> 0) & 0xF]);
}

uint8_t debug_getc() {
    // Wait for new data
    while(((dbg_rx_head - dbg_rx_tail)&0xFF)==0) {}

    // Read
    uint8_t x = dbg_rx_buf[dbg_rx_tail & 0x7F];
    dbg_rx_tail++;
    return x;
}

void debug_write(char *s) {
    for (; *s; s++) debug_putc(*s);
}

char *debug_readline() {
    for (size_t i = 0; i < sizeof(line_buf)-1; i++) {
        line_buf[i] = debug_getc();

        if (line_buf[i] == '\n') {
            line_buf[i] = '\0';
            break;
        }
    }
    line_buf[sizeof(line_buf)-1] = '\0';

    return line_buf;
}

void debug_printf(const char *fmt, ...) {
    va_list args;

    // Parse the arguments
    for (va_start(args, fmt); *fmt != '\0'; ++fmt) {
        // Is format specifier?
        if (*fmt == '%') {
            switch(*(++fmt)) {
                case 'h': {
                    uint32_t v = va_arg(args, int);

                    debug_puthex(v >> 24);
                    debug_puthex(v >> 16);
                    debug_puthex(v >> 8);
                    debug_puthex(v >> 0);

                    break;
                }

                case 's':
                    debug_write(va_arg(args, char *));
                    break;

                default:
                    debug_write("{UNK FORMAT}");
                    break;
            }
        } else {
            debug_putc(*fmt);
        }
    }
}

/* Dummy functions to make newlib happy so we can use vsprintf */
/*void _close(void) {}
void _lseek(void) {}
void _read(void) {}
void _write(void) {}
void _fstat(void) {}
void _getpid(void) {}
void _isatty(void) {}
void _kill(void) {}

void debug_printf(const char *fmt, ...) {
    char buf[100] = {0};
    va_list args;
    va_start(args, fmt);
    vsprintf(buf, fmt, args);

    char *b = buf;
    while (*b) {debug_putc(*b++);}
}*/

debug.h

#ifndef DEBUG_LINK
#define DEBUG_LINK

void debug_putc(uint8_t x);
void debug_puthex(uint8_t x);
uint8_t debug_getc();

void debug_write(char *s);
char *debug_readline();

void debug_printf(const char *fmt, ...);

#endif

debug_console.py

import openocd
import threading
import queue
import sys
from elftools.elf.elffile import ELFFile

# Get addresses
symtab = ELFFile(open("main.elf", "rb")).get_section_by_name(".symtab")

# Check if symbol addresses exist
dbg_tx_tail = symtab.get_symbol_by_name("dbg_tx_tail")
dbg_tx_head = symtab.get_symbol_by_name("dbg_tx_head")
dbg_tx_buf  = symtab.get_symbol_by_name("dbg_tx_buf")
dbg_rx_tail = symtab.get_symbol_by_name("dbg_rx_tail")
dbg_rx_head = symtab.get_symbol_by_name("dbg_rx_head")
dbg_rx_buf  = symtab.get_symbol_by_name("dbg_rx_buf")

if None in [dbg_tx_tail, dbg_tx_head, dbg_tx_buf, dbg_rx_tail, dbg_rx_head, dbg_rx_buf]:
    print("[-] One or more symbols do not exist!")
    exit(-1)

dbg_tx_tail = dbg_tx_tail[0].entry.st_value
dbg_tx_head = dbg_tx_head[0].entry.st_value
dbg_tx_buf  = dbg_tx_buf[0].entry.st_value
dbg_rx_tail = dbg_rx_tail[0].entry.st_value
dbg_rx_head = dbg_rx_head[0].entry.st_value
dbg_rx_buf  = dbg_rx_buf[0].entry.st_value

# Connect to openocd
oocd = openocd.OpenOcd()
oocd.connect()

write_queue  = queue.Queue()

def user_in():
    while True:
        x = input()
        write_queue.put((x + "\n").encode())

t = threading.Thread(target=user_in)
t.daemon = True
t.start()

# Socket loop
while True:
    # Read  
    tail = oocd.read_memory(dbg_tx_tail, 1, 8)[0]
    head = oocd.read_memory(dbg_tx_head, 1, 8)[0]

    fill = (head - tail) & 0xFF
    
    if fill != 0:
        # Get the latest value
        contig = 0x80 - (tail & 0x7F)
        if fill > contig:
            data = oocd.read_memory(dbg_tx_buf + (tail & 0x7F), contig, 8)
            sys.stdout.buffer.write(bytes(data))

            tail = (tail + contig) & 0xFF
            fill -= contig
        
        data = oocd.read_memory(dbg_tx_buf + (tail & 0x7F), fill, 8)
        sys.stdout.buffer.write(bytes(data))
        sys.stdout.flush()

        # Write back the new value
        tail = (tail + fill) & 0xFF
        oocd.write_memory(dbg_tx_tail, [tail], 8)

    # Write
    try:
        # Try and get data to write
        data = write_queue.get(block=False)

        for d in data:
            # Check if there's space
            while True:
                tail = oocd.read_memory(dbg_rx_tail, 1, 8)[0]
                head = oocd.read_memory(dbg_rx_head, 1, 8)[0]

                fill = (head - tail) & 0xFF

                if fill != 0x80:
                    oocd.write_memory(dbg_rx_buf + (head & 0x7F), [d], 8)

                    head = (head + 1) & 0xFF
                    oocd.write_memory(dbg_rx_head, [head], 8)
                    
                    break
    except queue.Empty:
        pass