Add jtag support in simulation via a socket

This adds a local socket that can be used to communicate with
the debug tool (which will be committed separately) and generates
the JTAG signals.

We generate the low level JTAG signals, thus directly driving the
simulated BSCANE2, and the Xilinx DTM

Signed-off-by: Benjamin Herrenschmidt <benh@kernel.crashing.org>

sim_jtag.vhdl

@@ -0,0 +1,105 @@
+library ieee;
+use ieee.std_logic_1164.all;
+use ieee.numeric_std.all;
+
+library work;
+use work.sim_jtag_socket.all;
+
+library unisim;
+use unisim.vcomponents.all;
+
+entity sim_jtag is
+end sim_jtag;
+
+architecture behaviour of sim_jtag is
+begin
+    jtag: process
+	-- Global JTAG signals (used by BSCANE2 inside dmi_dtm
+	alias j : glob_jtag_t is glob_jtag;
+
+	-- Super fast JTAG clock for sim. For debugging the JTAG module,
+	-- change this to something much larger, for example 60ns, to reflect
+	-- more realistic conditions.
+	constant jclk_period : time := 1 ns;
+
+	-- Polling the socket... this could be made slower when nothing
+	-- is connected once we have that indication from the C code.
+	constant poll_period : time := 100 ns;
+
+	-- Number of dummy JTAG clocks to inject after a command. (I haven't
+	-- got that working with UrJtag but at least with sim, having the
+	-- right number here allows the synchronizers time to complete a
+	-- command on the first message exchange, thus avoiding the need
+	-- for two full shifts for a response.
+	constant dummy_clocks : integer := 80;
+
+	procedure clock(count: in INTEGER) is
+	begin
+	    for i in 1 to count loop
+		j.tck <= '0';
+		wait for jclk_period/2;
+		j.tck <= '1';
+		wait for jclk_period/2;
+	    end loop;
+	end procedure clock;
+
+	procedure clock_command(cmd: in std_ulogic_vector;
+				rsp: out std_ulogic_vector) is
+	begin
+	    j.capture <= '1';
+	    clock(1);
+	    j.capture <= '0';
+	    clock(1);
+	    j.shift <= '1';
+	    for i in 0 to cmd'length-1 loop
+		j.tdi <= cmd(i);
+		rsp := rsp(1 to rsp'length-1) & j.tdo;
+		clock(1);
+	    end loop;
+	    j.shift <= '0';
+	    j.update <= '1';
+	    clock(1);
+	    j.update <= '0';
+	    clock(1);
+	end procedure clock_command;
+
+	variable cmd   : std_ulogic_vector(0 to 247);
+	variable rsp   : std_ulogic_vector(0 to 247);
+	variable msize : std_ulogic_vector(7 downto 0);
+	variable size  : integer;
+
+    begin
+
+	-- init & reset
+	j.reset <= '1';
+	j.sel <= "0000";
+	j.capture <= '0';
+	j.update <= '0';
+	j.shift <= '0';
+	j.tdi <= '0';
+	j.tms <= '0';
+	j.runtest <= '0';
+	clock(5);
+	j.reset <= '0';
+	clock(5);
+
+	-- select chain USER2
+	-- XXX TODO: Send that via protocol instead
+	-- XXX TODO: Also maybe have the C code tell us if connected or not
+	-- and clock when connected.
+	j.sel <= "0010";
+	clock(1);
+	rsp := (others => '0');
+	while true loop
+	    wait for poll_period;
+	    sim_jtag_read_msg(cmd, msize);
+	    size := to_integer(unsigned(msize));
+	    if size /= 0 and size < 248 then
+		clock_command(cmd(0 to size-1),
+			      rsp(0 to size-1));
+		sim_jtag_write_msg(rsp, msize);
+		clock(dummy_clocks);
+	    end if;
+	end loop;
+    end process;    
+end;

sim_jtag_socket.vhdl

@@ -0,0 +1,24 @@
+library ieee;
+use ieee.std_logic_1164.all;
+
+package sim_jtag_socket is
+    procedure sim_jtag_read_msg(out_msg  : out std_ulogic_vector(247 downto 0);
+				out_size : out std_ulogic_vector(7 downto 0));
+    attribute foreign of sim_jtag_read_msg : procedure is "VHPIDIRECT sim_jtag_read_msg";
+    procedure sim_jtag_write_msg(in_msg  : in std_ulogic_vector(247 downto 0);
+				 in_size : in std_ulogic_vector(7 downto 0));
+    attribute foreign of sim_jtag_write_msg : procedure is "VHPIDIRECT sim_jtag_write_msg";
+end sim_jtag_socket;
+
+package body sim_jtag_socket is
+    procedure sim_jtag_read_msg(out_msg  : out std_ulogic_vector(247 downto 0);
+				out_size : out std_ulogic_vector(7 downto 0)) is
+    begin
+	assert false report "VHPI" severity failure;
+    end sim_jtag_read_msg;
+    procedure sim_jtag_write_msg(in_msg  : in std_ulogic_vector(247 downto 0);
+				 in_size : in std_ulogic_vector(7 downto 0)) is
+    begin
+	assert false report "VHPI" severity failure;
+    end sim_jtag_write_msg;
+end sim_jtag_socket;

sim_jtag_socket_c.c

@@ -0,0 +1,222 @@
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <poll.h>
+#include <signal.h>
+#include <fcntl.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+
+/* XXX Make that some parameter */
+#define TCP_PORT	13245
+#define MAX_PACKET	32
+
+#define vhpi0	2	/* forcing 0 */
+#define vhpi1	3	/* forcing 1 */
+
+static void to_std_logic_vector(unsigned long val, unsigned char *p,
+				unsigned long len)
+{
+	if (len > 64) {
+		fprintf(stderr, "%s: invalid length %lu\n", __func__, len);
+		exit(1);
+	}
+
+	for (unsigned long i = 0; i < len; i++) {
+		if ((val >> (len-1-i) & 1))
+			*p = vhpi1;
+		else
+			*p = vhpi0;
+
+		p++;
+	}
+}
+
+static uint64_t from_std_logic_vector(unsigned char *p, unsigned long len)
+{
+	unsigned long ret = 0;
+
+	if (len > 64) {
+		fprintf(stderr, "%s: invalid length %lu\n", __func__, len);
+		exit(1);
+	}
+
+	for (unsigned long i = 0; i < len; i++) {
+		unsigned char bit;
+
+		if (*p == vhpi0) {
+			bit = 0;
+		} else if (*p == vhpi1) {
+			bit = 1;
+		} else {
+			fprintf(stderr, "%s: bad bit %d\n", __func__, *p);
+			bit = 0;
+		}
+
+		ret = (ret << 1) | bit;
+		p++;
+	}
+
+	return ret;
+}
+
+static int fd = -1;
+static int cfd = -1;
+
+static void open_socket(void)
+{
+	struct sockaddr_in addr;
+	int opt, rc, flags;
+
+	if (fd >= 0 || fd < -1)
+		return;
+
+	signal(SIGPIPE, SIG_IGN);
+	fd = socket(AF_INET, SOCK_STREAM, 0);
+	if (fd < 0) {
+		fprintf(stderr, "Failed to open debug socket !\r\n");
+		goto fail;
+	}
+
+	rc = 0;
+	flags = fcntl(fd, F_GETFL);
+	if (flags >= 0)
+		rc = fcntl(fd, F_SETFL, flags | O_NONBLOCK);
+	if (flags < 0 || rc < 0) {
+		fprintf(stderr, "Failed to configure debug socket !\r\n");
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.sin_family = AF_INET;
+	addr.sin_port = htons(TCP_PORT);
+	addr.sin_addr.s_addr = htonl(INADDR_ANY);
+	opt = 1;
+	setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
+	rc = bind(fd, (struct sockaddr *)&addr, sizeof(addr));
+	if (rc < 0) {
+		fprintf(stderr, "Failed to bind debug socket !\r\n");
+		goto fail;
+	}
+	rc = listen(fd,1);
+	if (rc < 0) {
+		fprintf(stderr, "Failed to listen to debug socket !\r\n");
+		goto fail;
+	}
+	fprintf(stderr, "Debug socket ready\r\n");
+	return;
+fail:
+	if (fd >= 0)
+		close(fd);
+	fd = -2;
+}
+
+static void check_connection(void)
+{
+	struct sockaddr_in addr;
+	socklen_t addr_len = sizeof(addr);
+
+	cfd = accept(fd, (struct sockaddr *)&addr, &addr_len);
+	if (cfd < 0)
+		return;
+	fprintf(stderr, "Debug client connected !\r\n");
+}
+
+void sim_jtag_read_msg(unsigned char *out_msg, unsigned char *out_size)
+{
+	unsigned char data[MAX_PACKET];
+	unsigned char size = 0;
+	struct pollfd fdset[1];
+	int rc, i;
+
+	if (fd == -1)
+		open_socket();
+	if (fd < 0)
+		goto finish;
+	if (cfd < 0)
+		check_connection();
+	if (cfd < 0)
+		goto finish;
+
+	memset(fdset, 0, sizeof(fdset));
+	fdset[0].fd = cfd;
+	fdset[0].events = POLLIN;
+	rc = poll(fdset, 1, 0);
+	if (rc <= 0)
+		goto finish;
+	rc = read(cfd, data, MAX_PACKET);
+	if (rc < 0)
+		fprintf(stderr, "Debug read error, assuming client disconnected !\r\n");
+	if (rc == 0)
+		fprintf(stderr, "Debug client disconnected !\r\n");
+	if (rc <= 0) {
+		close(cfd);
+		cfd = -1;
+		goto finish;
+	}
+
+#if 0
+	fprintf(stderr, "Got message:\n\r");
+	{
+		for (i=0; i<rc; i++)
+			fprintf(stderr, "%02x ", data[i]);
+		fprintf(stderr, "\n\r");
+	}
+#endif
+	size = data[0]; /* Size in bits */
+
+	/* Special sizes */
+	if (size == 255) {
+		/* JTAG reset, message to translate */
+		goto finish;
+	}
+
+	if (((rc - 1) * 8) < size) {
+		fprintf(stderr, "Debug short read: %d bytes for %d bits, truncating\r\n",
+			rc - 1, size);
+		size = (rc - 1) * 8;
+	}
+
+	for (i = 0; i < size; i++) {
+		int byte = i >> 3;
+		int bit = 1 << (i & 7);
+		out_msg[i] = (data[byte+1] & bit) ? vhpi1 : vhpi0;
+	}
+finish:
+	to_std_logic_vector(size, out_size, 8);
+}
+
+void sim_jtag_write_msg(unsigned char *in_msg, unsigned char *in_size)
+{
+	unsigned char data[MAX_PACKET];
+	unsigned char size;
+	int rc, i;
+
+	size = from_std_logic_vector(in_size, 8);
+	data[0] = size;
+	for (i = 0; i < size; i++) {
+		int byte = i >> 3;
+		int bit = 1 << (i & 7);
+		if (in_msg[i] == vhpi1)
+			data[byte+1] |= bit;
+		else
+			data[byte+1] &= ~bit;
+	}
+	rc = (size + 7) / 8;
+
+#if 0
+	fprintf(stderr, "Sending response:\n\r");
+	{
+		for (i=0; i<rc; i++)
+			fprintf(stderr, "%02x ", data[i]);
+		fprintf(stderr, "\n\r");
+	}
+#endif
+
+	rc = write(cfd, data, rc);
+	if (rc < 0)
+		fprintf(stderr, "Debug write error, ignoring\r\n");
+}
+