add missing fileszap

3 files changed, 764 insertions, 0 deletions

diff --git a/zap-phy.c b/zap-phy.c
new file mode 100644
index 0000000..c4dc7eb
--- /dev/null
+++ b/zap-phy.c
@@ -0,0 +1,172 @@
+#include <curses.h>
+#include <errno.h>
+#include <limits.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/fcntl.h>
+#include <unistd.h>
+
+#include "zap.h"
+
+extern int mdio_read(long ioaddr, int phy_id, int mii_reg_num);
+extern void mdio_write(int ioaddr, int phy_id, int location, int value);
+
+struct zap_phy {
+	long ioaddr;
+	int phy_id;
+	int devad;
+	int irq_fd;
+	char aux_buf[16];
+};
+
+static bool phy_read(void *private, u32 addr, u32 *val)
+{
+	struct zap_phy *phy = private;
+	int v;
+
+	v = mdio_read(phy->ioaddr, phy->phy_id | phy->devad, addr);
+
+	*val = v >= 0 ? v : 0;
+
+	return v >= 0;
+}
+
+static void phy_write(void *private, u32 addr, u32 val)
+{
+	struct zap_phy *phy = private;
+
+	mdio_write(phy->ioaddr, phy->phy_id | phy->devad, addr, val);
+}
+
+static void phy_c45_key(struct zap_private *priv, void *private, int key)
+{
+	struct zap_phy *phy = private;
+	int val;
+
+	switch (key) {
+	case 'r':
+		val = mdio_read(phy->ioaddr, phy->phy_id | 7, 0);
+		if (val < 0)
+			return;
+		mdio_write(phy->ioaddr, phy->phy_id | 7, 0, val | 0x200);
+		break;
+
+	case 'p':
+		zap_prompt_input(priv, 'p', "PHY devad");
+		break;
+	}
+}
+
+static void phy_c45_print_unit(void *private)
+{
+	struct zap_phy *phy = private;
+	const char *dev = "";
+
+	switch (phy->devad) {
+	case 1: dev = " PMA/PMD";	break;
+	case 2: dev = " WIS";		break;
+	case 3: dev = " PCS";		break;
+	case 4: dev = " PHYXS";		break;
+	case 5: dev = " DTEXS";		break;
+	case 7: dev = " AN";		break;
+	}
+
+	printw("PHY #%u.%-2u%s", (phy->phy_id >> 5) & 31, phy->devad, dev);
+	clrtoeol();
+}
+
+static void phy_c45_update_aux(void *private)
+{
+	struct zap_phy *phy = private;
+	char new[16];
+	int len;
+
+	if (phy->irq_fd == -1)
+		return;
+
+	lseek(phy->irq_fd, 0, SEEK_SET);
+	len = read(phy->irq_fd, new, sizeof(new));
+	if (len < 0 || len >= sizeof(new))
+		return;
+
+	len -= 1;
+
+	if (!memcmp(phy->aux_buf, new, len))
+		return;
+
+	memcpy(phy->aux_buf, new, len);
+	mvprintw(1, 41, "IRQ ");
+	mvaddnstr(1, 45, new, len);
+	clrtoeol();
+}
+
+static void phy_c45_input(void *private, char mode, const char *buf)
+{
+	struct zap_phy *phy = private;
+	unsigned long unit;
+
+	errno = 0;
+	unit = strtoul(buf, NULL, 0);
+	if (unit == ULONG_MAX && errno == ERANGE)
+		return;
+	if (unit > 31)
+		return;
+
+	phy->devad = unit;
+}
+
+static const struct zap phy_c45_zap = {
+	.val_width = 4,
+	.max_addr = 0xffff,
+	.read = phy_read,
+	.write = phy_write,
+	.key = phy_c45_key,
+	.print_header = phy_c45_print_unit,
+	.update_aux = phy_c45_update_aux,
+	.input = phy_c45_input,
+};
+
+static void phy_print_c22_unit(void *private)
+{
+	struct zap_phy *phy = private;
+
+	printw("PHY #%u", phy->phy_id);
+}
+
+static const struct zap phy_c22_zap = {
+	.val_width = 4,
+	.max_addr = 0x1f,
+	.read = phy_read,
+	.write = phy_write,
+	.print_header = phy_print_c22_unit,
+};
+
+
+void phy_zap(long ioaddr, int phy_id, unsigned long start)
+{
+	const struct zap *z;
+	struct zap_phy phy = {
+		.ioaddr = ioaddr,
+		.irq_fd = -1,
+	};
+
+	if (phy_id & 0x8000) {
+		phy.phy_id = phy_id & ~31;
+		phy.devad = phy_id & 31;
+
+		if (phy_id == 0x8000)
+			phy.irq_fd = open("/sys/class/gpio/gpio82/value", O_RDONLY);
+		else if (phy_id == 0x8000 + 8 * 32)
+			phy.irq_fd = open("/sys/class/gpio/gpio81/value", O_RDONLY);
+
+		z = &phy_c45_zap;
+	} else {
+		phy.phy_id = phy_id;
+		phy.devad = 0;
+
+		z = &phy_c22_zap;
+	}
+	if (start >= z->max_addr)
+		start = z->max_addr;
+	zap(z, start, &phy);
+}
diff --git a/zap.c b/zap.c
new file mode 100644
index 0000000..2782793
--- /dev/null
+++ b/zap.c
@@ -0,0 +1,561 @@
+#include <curses.h>
+#include <ctype.h>
+#include <errno.h>
+#include <limits.h>
+#include <signal.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/fcntl.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include "zap.h"
+
+struct zap_private {
+	WINDOW		*scr;
+	WINDOW		*regwin;
+	const struct zap *zap;
+	void		*private;
+	unsigned char	width;
+	unsigned char	height;
+	unsigned char	x_addr;
+	unsigned char	w_addr;
+	unsigned char	x_val;
+	unsigned char	w_val;
+	unsigned char	u_val;
+	unsigned char	x_ascii;
+	unsigned char	w_ascii;
+	unsigned char	u_ascii;
+	unsigned char	w_mask;
+	unsigned char	l2_w;
+#define F_REDRAW_ADDR	1
+#define F_REDRAW_VALS	2
+	unsigned char	flags;
+	char		mode;
+	int		x_curs;
+	int		y_curs;
+	u32		v_mask;
+	unsigned long	addr;
+	u32		*vals;
+	unsigned int	ipidx;
+	char		ipbuf[16];
+};
+
+static void zap_quit(int signr)
+{
+	if (!isendwin())
+		endwin();
+	signal(signr, SIG_DFL);
+	raise(signr);
+}
+
+static int fls(u32 val)
+{
+	int res = 32;
+	if (!val)
+		return 0;
+#define T(v,r,bits) \
+	if (!(v & (~0 << (32 - bits)))) { \
+		v <<= bits; \
+		r -= bits; \
+	}
+	T(val, res, 16)
+	T(val, res, 8)
+	T(val, res, 4)
+	T(val, res, 2)
+	T(val, res, 1)
+#undef T
+	return res;
+}
+
+static void dashes(int y, int x, int n)
+{
+	const char *dashes = "=========================================";
+
+	attron(COLOR_PAIR(3));
+	mvaddnstr(y, x, dashes, n);
+	attroff(COLOR_PAIR(3));
+}
+
+static void dashes_centered(int y, int x, int w, const char *text)
+{
+	int text_w = strlen(text);
+	int dashes_w = w - text_w;
+	int dashes_l = dashes_w / 2;
+	int dashes_r = w - dashes_l - text_w;
+
+	dashes(y, x, dashes_l);
+	attron(COLOR_PAIR(4));
+	mvaddstr(0, x + dashes_l, text);
+	attroff(COLOR_PAIR(4));
+	dashes(y, x + dashes_l + text_w, dashes_r); 
+}
+
+static void zap_header(const struct zap_private *priv)
+{
+	attron(COLOR_PAIR(4));
+	mvaddstr(0, priv->x_addr, "Addr");
+	attroff(COLOR_PAIR(4));
+	dashes_centered(0, priv->x_val, priv->w_val, " Hex Value ");
+
+	if (priv->x_ascii)
+		dashes_centered(0, priv->x_ascii, priv->w_ascii, " ASCII ");
+}
+
+static void zap_write(const struct zap_private *priv, unsigned long val)
+{
+	const struct zap *zap = priv->zap;
+
+	if (zap->write)
+		zap->write(priv->private, priv->addr << priv->zap->addr_shift,
+		   	   val);
+}
+
+static void zap_sub_header(const struct zap_private *priv)
+{
+	const struct zap *zap = priv->zap;
+
+	if (zap->print_header) {
+		attron(COLOR_PAIR(2));
+		move(1, priv->x_val);
+		zap->print_header(priv->private);
+		attroff(COLOR_PAIR(2));
+	}
+}
+
+static void zap_input(struct zap_private *priv)
+{
+	const struct zap *zap = priv->zap;
+
+	if (zap->input)
+		zap->input(priv->private, priv->mode, priv->ipbuf);
+}
+
+static void zap_key(struct zap_private *priv, int ch)
+{
+	const struct zap *zap = priv->zap;
+
+	if (zap->key)
+		zap->key(priv, priv->private, ch);
+}
+
+static void zap_init(struct zap_private *priv, const struct zap *zap, void *private)
+{
+	unsigned int width, n;
+
+	memset(priv, 0, sizeof(*priv));
+
+	priv->zap = zap;
+	priv->private = private;
+	priv->mode = '\0';
+	priv->flags = F_REDRAW_ADDR | F_REDRAW_VALS;
+	priv->ipidx = 0;
+
+	priv->v_mask = (1 << (4 * zap->val_width)) - 1;
+	priv->w_addr = (fls(zap->max_addr) + 3) / 4;
+	priv->u_val = zap->val_width + 1;
+	priv->u_ascii = zap->val_width / 2;
+
+	priv->scr = initscr();
+	getmaxyx(priv->scr, priv->height, priv->width);
+	priv->x_curs = 0;
+	priv->y_curs = priv->height - 1;
+
+	start_color();
+	init_pair(1, COLOR_GREEN, COLOR_BLACK);
+	init_pair(2, COLOR_YELLOW, COLOR_BLACK);
+	init_pair(3, COLOR_MAGENTA, COLOR_BLACK);
+	init_pair(4, COLOR_CYAN, COLOR_BLACK);
+	cbreak();
+	noecho();
+	nodelay(priv->scr, TRUE);
+	keypad(priv->scr, TRUE);
+
+	priv->x_addr = 1;
+	priv->x_val = priv->x_addr + priv->w_addr + 2;
+
+	width = priv->width - priv->x_val;
+	if (zap->ascii) {
+		width -= 2;
+		width /= priv->u_val + priv->u_ascii;
+	} else {
+		width /= priv->u_val;
+	}
+
+	priv->l2_w = fls(width) - 1;
+
+	n = 1 << priv->l2_w;
+
+	priv->w_mask = n - 1;
+	priv->w_val = priv->u_val * n - 1;
+	priv->w_ascii = priv->u_ascii * n;
+
+//	priv->vals = calloc(sizeof(*priv->vals), n);
+
+	if (zap->ascii) {
+		priv->x_ascii = priv->x_val + priv->w_val + 2;
+		width = priv->x_ascii + priv->w_ascii;
+	} else {
+		width = priv->x_val + priv->w_val;
+	}
+
+	priv->regwin = subwin(priv->scr, priv->height - 3, width, 2, 0);
+	syncok(priv->regwin, FALSE);
+
+	zap_header(priv);
+	zap_sub_header(priv);
+}
+
+static void move_ifnot(int y, int x)
+{
+	int curx, cury;
+
+	getyx(stdscr, curx, cury);
+	if (curx != x || cury != y)
+		move(y, x);
+}
+
+void zap_prompt_input(struct zap_private *priv, char mode, const char *prompt)
+{
+	if (priv->mode == '\0') {
+		priv->mode = mode;
+
+		mvprintw(priv->height - 1, 0, "%s: ", prompt);
+		clrtoeol();
+		curs_set(1);
+	}
+}
+
+static void zap_prompt_clean(struct zap_private *priv)
+{
+	priv->ipidx = 0;
+	priv->mode = '\0';
+
+	move(priv->height - 1, 0);
+	clrtoeol();
+	curs_set(0);
+}
+
+static char ascii(unsigned int val)
+{
+	return val >= 127 || !isprint(val) ? '.' : val;
+}
+
+static void zap_format_val(const struct zap_private *priv, int line, int col,
+			   u32 val, bool ok)
+{
+	unsigned int val_width = priv->u_val - 1;
+	int i, x = priv->x_val + priv->u_val * col;
+	char buf[8];
+
+	if (ok)
+		mvwprintw(priv->regwin, line, x, "%.*x ", val_width, val);
+	else
+		mvwprintw(priv->regwin, line, x, "%.*s ", val_width,
+			  "xxxxxxxx");
+
+	if (priv->x_ascii) {
+		for (i = 0; i < priv->u_ascii && i < sizeof(buf) - 1;
+		     i++, val >>= 8)
+			buf[i] = ascii(val & 255);
+
+		x = priv->x_ascii + priv->u_ascii * col;
+		mvwaddnstr(priv->regwin, line, x, buf, i);
+	}
+}
+
+static void zap_set_addr(struct zap_private *priv, unsigned long addr)
+{
+	if (priv->addr != addr) {
+		priv->addr = addr;
+		priv->flags |= F_REDRAW_ADDR;
+	}
+}
+
+static void zap_handle_key(struct zap_private *priv, int ch)
+{
+	unsigned long val;
+
+	switch (ch) {
+	case 12:
+		wrefresh(curscr);
+		break;
+
+	case '0'...'9':
+	case 'a'...'f':
+		if (priv->ipidx == 0 && priv->mode == '\0')
+			zap_prompt_input(priv, 'c', "New value");
+		if (priv->ipidx < sizeof(priv->ipbuf) - 1) {
+			priv->ipbuf[priv->ipidx++] = ch;
+			addch(ch);
+		}
+		break;
+
+	case 'g':
+		zap_prompt_input(priv, 'g', "New addr");
+		break;
+
+	case KEY_BACKSPACE:
+	case 8:
+		if (priv->ipidx > 0) {
+			priv->ipidx -= 1;
+			addch(8);
+			clrtoeol();
+		}
+		break;
+
+	case KEY_ENTER:
+	case '\n':
+		if (priv->ipidx == 0) {
+			zap_prompt_clean(priv);
+			break;
+		}
+
+		priv->ipbuf[priv->ipidx] = '\0';
+
+		switch (priv->mode) {
+		case 'c':
+			errno = 0;
+			val = strtoul(priv->ipbuf, NULL, 16);
+			if (val != ULONG_MAX || errno != ERANGE)
+				zap_write(priv, val);
+			break;
+
+		case 'g':
+			errno = 0;
+			val = strtoul(priv->ipbuf, NULL, 16);
+			if (val != ULONG_MAX || errno != ERANGE)
+				zap_set_addr(priv, val >>
+					     priv->zap->addr_shift);
+			break;
+
+		default:
+			zap_input(priv);
+			zap_sub_header(priv);
+			break;
+		}
+		zap_prompt_clean(priv);
+		break;
+
+	default:
+		zap_key(priv, ch);
+		break;
+	}
+}
+
+void zap(const struct zap *zap, unsigned long start, void *private)
+{
+	struct zap_private priv;
+	unsigned int page_offset, offset, page, hly;
+	u32 regs[192 * 2];
+	bool oks[192 * 2];
+	bool quit = false;
+	int ch;
+
+	signal(SIGHUP, zap_quit);
+	signal(SIGINT, zap_quit);
+	signal(SIGQUIT, zap_quit);
+	signal(SIGTSTP, zap_quit);
+	signal(SIGTTIN, zap_quit);
+	signal(SIGTTOU, zap_quit);
+
+	zap_init(&priv, zap, private);
+
+	/* page = how many "addresses" we can fit on a page */
+	page = (priv.height - 3) << priv.l2_w;
+	/* offset = offset of the left-hand side of the highlighted line */
+	offset = ((page - 1) / 2) & ~priv.w_mask;
+	/* hly = highlighted line y offset */
+	hly = offset >> priv.l2_w;
+	/* how much to page by */
+	page_offset = 0x100 >> zap->addr_shift;
+
+	priv.addr = start >> zap->addr_shift;
+
+	do {
+		unsigned int cur_left = priv.addr & ~priv.w_mask;
+		unsigned int top_left = cur_left - offset;
+		unsigned int i, flags;
+		int attr = 0;
+
+		flags = priv.flags;
+		priv.flags = 0;
+
+		if (flags & F_REDRAW_ADDR) {
+			curs_set(0);
+
+			for (i = 0; i < page; i += priv.w_mask + 1) {
+				unsigned int line = i >> priv.l2_w;
+				unsigned int addr = top_left + i;
+
+				if (addr < zap->min_addr ||
+				    addr > zap->max_addr) {
+					wmove(priv.regwin, line, 0);
+					wclrtoeol(priv.regwin);
+					continue;
+				}
+
+				if (line == hly)
+					wattron(priv.regwin, COLOR_PAIR(1));
+				mvwprintw(priv.regwin, line, priv.x_addr, "%*.*x",
+					  priv.w_addr, priv.w_addr,
+					  addr << zap->addr_shift);
+				if (line == hly)
+					wattroff(priv.regwin, COLOR_PAIR(1));
+			}
+		}
+
+		for (i = 0; i < page; i++) {
+			unsigned int line = i >> priv.l2_w;
+			unsigned int addr = top_left + i;
+			unsigned int col = i & priv.w_mask;
+			unsigned int mask;
+			int new_attr;
+			bool ok;
+			u32 val;
+
+			if (addr < zap->min_addr ||
+			    addr > zap->max_addr) {
+				oks[i] = false;
+				continue;
+			}
+
+			if (line == hly) {
+				/* The highlight line must be redrawn on
+				 * address changes
+				 */
+				mask = F_REDRAW_VALS | F_REDRAW_ADDR;
+
+				if (addr == priv.addr)
+					new_attr = COLOR_PAIR(2);
+				else
+					new_attr = COLOR_PAIR(1);
+			} else {
+				mask = F_REDRAW_VALS;
+				new_attr = 0;
+			}
+
+			if (zap->read_block) {
+				if (col == 0) {
+					ok = zap->read_block(private,
+							     addr << zap->addr_shift,
+							     priv.vals,
+							     priv.w_mask + 1);
+				} else {
+					ok = 1;
+				}
+				val = priv.vals[col];
+			} else {
+				ok = 0;
+			}
+
+			if (!ok)
+				ok = zap->read(private, addr << zap->addr_shift,
+					       &val);
+			if (!ok)
+				val = 0;
+			val &= priv.v_mask;
+
+			if (flags & mask || ok != oks[i] || val != regs[i]) {
+				regs[i] = val;
+				oks[i] = ok;
+
+				if (attr != new_attr) {
+					if (new_attr)
+						wattron(priv.regwin, new_attr);
+					else
+						wattroff(priv.regwin, attr);
+					attr = new_attr;
+				}
+
+				zap_format_val(&priv, line, col, val, ok);
+			}
+		}
+
+		if (attr) {
+			wattroff(priv.regwin, attr);
+			attr = 0;
+		}
+
+		wsyncup(priv.regwin);
+
+		if (zap->update_aux) {
+			attron(COLOR_PAIR(2));
+			zap->update_aux(private);
+			attroff(COLOR_PAIR(2));
+		}
+
+		/* Restore the cursor position */
+		move_ifnot(priv.y_curs, priv.x_curs);
+		if (priv.mode && flags & F_REDRAW_ADDR)
+			curs_set(1);
+
+		ch = getch();
+		switch (ch) {
+		case KEY_LEFT:
+		case 'h':
+			if (priv.addr > zap->min_addr)
+				zap_set_addr(&priv, priv.addr - 1);
+			break;
+
+		case KEY_RIGHT:
+		case 'l':
+			if (priv.addr < zap->max_addr)
+				zap_set_addr(&priv, priv.addr + 1);
+			break;
+
+		case KEY_UP:
+		case 'k':
+			if (priv.addr >= priv.w_mask + 1)
+				zap_set_addr(&priv, priv.addr - (priv.w_mask + 1));
+			break;
+
+		case KEY_DOWN:
+		case 'j':
+			if (priv.addr < zap->max_addr - priv.w_mask)
+				zap_set_addr(&priv, priv.addr + priv.w_mask + 1);
+			break;
+
+		case KEY_PPAGE:
+			if (priv.addr >= zap->min_addr + page_offset) {
+				zap_set_addr(&priv, priv.addr - page_offset);
+				break;
+			}
+		case KEY_HOME:
+			if (priv.addr & ~priv.w_mask)
+				zap_set_addr(&priv, (priv.addr & priv.w_mask) +
+					     (zap->min_addr & ~priv.w_mask));
+			break;
+
+		case KEY_NPAGE:
+			if (priv.addr < zap->max_addr - page_offset) {
+				zap_set_addr(&priv, priv.addr + page_offset);
+				break;
+			}
+		case KEY_END:
+			if (priv.addr < zap->max_addr - priv.w_mask)
+				zap_set_addr(&priv, (priv.addr & priv.w_mask) +
+					       zap->max_addr - priv.w_mask);
+			break;
+
+		case ERR:
+			break;
+
+		case 'q':
+			quit = true;
+			break;
+
+		default:
+			zap_handle_key(&priv, ch);
+			break;
+		}
+
+		/* Save the current cursor position */
+		getyx(priv.scr, priv.x_curs, priv.y_curs);
+
+		usleep(40000);
+	} while (!quit);
+
+	endwin();
+}
diff --git a/zap.h b/zap.h
new file mode 100644
index 0000000..cd835c0
--- /dev/null
+++ b/zap.h
@@ -0,0 +1,31 @@
+#ifndef ZAP_H
+#define ZAP_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+typedef uint32_t u32;
+typedef uint16_t u16;
+typedef uint8_t u8;
+
+struct zap_private;
+
+struct zap {
+	unsigned int	val_width;
+	unsigned int	addr_shift;
+	unsigned int	min_addr;
+	unsigned int	max_addr;
+	bool		ascii;
+	bool (*read)(void *private, u32 addr, u32 *val);
+	bool (*read_block)(void *private, u32 addr, u32 *vals, size_t num);
+	void (*write)(void *private, u32 addr, u32 val);
+	void (*key)(struct zap_private *priv, void *private, int key);
+	void (*print_header)(void *private);
+	void (*update_aux)(void *private);
+	void (*input)(void *private, char mode, const char *str);
+};
+
+void zap_prompt_input(struct zap_private *priv, char mode, const char *prompt);
+void zap(const struct zap *zap, unsigned long start, void *private);
+
+#endif