MFH

Sponsored by:	The FreeBSD Foundation

4 files changed, 1027 insertions, 0 deletions

diff --git a/sys/riscv/htif/htif.c b/sys/riscv/htif/htif.c
new file mode 100644
index 0000000..08e6a43
--- /dev/null
+++ b/sys/riscv/htif/htif.c
@@ -0,0 +1,284 @@
+/*-
+ * Copyright (c) 2015 Ruslan Bukin <br@bsdpad.com>
+ * All rights reserved.
+ *
+ * Portions of this software were developed by SRI International and the
+ * University of Cambridge Computer Laboratory under DARPA/AFRL contract
+ * FA8750-10-C-0237 ("CTSRD"), as part of the DARPA CRASH research programme.
+ *
+ * Portions of this software were developed by the University of Cambridge
+ * Computer Laboratory as part of the CTSRD Project, with support from the
+ * UK Higher Education Innovation Fund (HEIF).
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/kernel.h>
+#include <sys/module.h>
+#include <sys/rman.h>
+#include <sys/pcpu.h>
+#include <sys/proc.h>
+
+#include <vm/vm.h>
+#include <vm/pmap.h>
+
+#include <dev/fdt/fdt_common.h>
+#include <dev/ofw/openfirm.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include <machine/bus.h>
+#include <machine/cpu.h>
+#include <machine/intr.h>
+#include <machine/asm.h>
+#include <machine/trap.h>
+#include <machine/vmparam.h>
+
+#include "htif.h"
+
+static struct resource_spec htif_spec[] = {
+	{ SYS_RES_IRQ,		0,	RF_ACTIVE },
+	{ -1, 0 }
+};
+
+struct intr_entry {
+	void (*func) (void *, uint64_t);
+	void *arg;
+};
+
+struct intr_entry intrs[HTIF_NDEV];
+
+uint64_t
+htif_command(uint64_t arg)
+{
+
+	return (machine_command(ECALL_HTIF_CMD, arg));
+}
+
+int
+htif_setup_intr(int id, void *func, void *arg)
+{
+
+	if (id >= HTIF_NDEV)
+		return (-1);
+
+	intrs[id].func = func;
+	intrs[id].arg = arg;
+
+	return (0);
+}
+
+static void
+htif_handle_entry(struct htif_softc *sc)
+{
+	uint64_t entry;
+	uint8_t devcmd;
+	uint8_t devid;
+
+	entry = machine_command(ECALL_HTIF_GET_ENTRY, 0);
+	while (entry) {
+		devid = HTIF_DEV_ID(entry);
+		devcmd = HTIF_DEV_CMD(entry);
+
+		if (devcmd == HTIF_CMD_IDENTIFY) {
+			/* Enumeration interrupt */
+			if (devid == sc->identify_id)
+				sc->identify_done = 1;
+		} else {
+			/* Device interrupt */
+			if (intrs[devid].func != NULL)
+				intrs[devid].func(intrs[devid].arg, entry);
+		}
+
+		entry = machine_command(ECALL_HTIF_GET_ENTRY, 0);
+	}
+}
+
+static int
+htif_intr(void *arg)
+{
+	struct htif_softc *sc;
+
+	sc = arg;
+
+	htif_handle_entry(sc);
+
+	csr_clear(sip, SIE_SSIE);
+
+	return (FILTER_HANDLED);
+}
+
+static int
+htif_add_device(struct htif_softc *sc, int i, char *id, char *name)
+{
+	struct htif_dev_ivars *di;
+
+	di = malloc(sizeof(struct htif_dev_ivars), M_DEVBUF, M_WAITOK | M_ZERO);
+	di->sc = sc;
+	di->index = i;
+	di->id = malloc(HTIF_ID_LEN, M_DEVBUF, M_WAITOK | M_ZERO);
+	memcpy(di->id, id, HTIF_ID_LEN);
+
+	di->dev = device_add_child(sc->dev, name, -1);
+	device_set_ivars(di->dev, di);
+
+	return (0);
+}
+
+static int
+htif_enumerate(struct htif_softc *sc)
+{
+	char id[HTIF_ID_LEN] __aligned(HTIF_ALIGN);
+	uint64_t paddr;
+	uint64_t data;
+	uint64_t cmd;
+	int len;
+	int i;
+
+	device_printf(sc->dev, "Enumerating devices\n");
+
+	for (i = 0; i < HTIF_NDEV; i++) {
+		paddr = pmap_kextract((vm_offset_t)&id);
+		data = (paddr << IDENTIFY_PADDR_SHIFT);
+		data |= IDENTIFY_IDENT;
+
+		sc->identify_id = i;
+		sc->identify_done = 0;
+
+		cmd = i;
+		cmd <<= HTIF_DEV_ID_SHIFT;
+		cmd |= (HTIF_CMD_IDENTIFY << HTIF_CMD_SHIFT);
+		cmd |= data;
+
+		htif_command(cmd);
+
+		/* Do poll as interrupts are disabled yet */
+		while (sc->identify_done == 0) {
+			htif_handle_entry(sc);
+		}
+
+		len = strnlen(id, sizeof(id));
+		if (len <= 0) {
+			continue;
+		}
+
+		if (bootverbose)
+			printf(" %d %s\n", i, id);
+
+		if (strncmp(id, "disk", 4) == 0)
+			htif_add_device(sc, i, id, "htif_blk");
+		else if (strncmp(id, "bcd", 3) == 0)
+			htif_add_device(sc, i, id, "htif_console");
+		else if (strncmp(id, "syscall_proxy", 13) == 0)
+			htif_add_device(sc, i, id, "htif_syscall_proxy");
+	}
+
+	return (bus_generic_attach(sc->dev));
+}
+
+int
+htif_read_ivar(device_t dev, device_t child, int which, uintptr_t *result)
+{
+	struct htif_dev_ivars *ivars;
+
+	ivars = device_get_ivars(child);
+
+	switch (which) {
+	case HTIF_IVAR_INDEX:
+                *result = ivars->index;
+		break;
+	case HTIF_IVAR_ID:
+		*result = (uintptr_t)ivars->id;
+	default:
+		return (EINVAL);
+	}
+
+	return (0);
+}
+
+static int
+htif_probe(device_t dev)
+{
+
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+
+	if (!ofw_bus_is_compatible(dev, "riscv,htif"))
+		return (ENXIO);
+
+	device_set_desc(dev, "HTIF bus device");
+	return (BUS_PROBE_DEFAULT);
+}
+
+static int
+htif_attach(device_t dev)
+{
+	struct htif_softc *sc;
+	int error;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+
+	if (bus_alloc_resources(dev, htif_spec, sc->res)) {
+		device_printf(dev, "could not allocate resources\n");
+		return (ENXIO);
+	}
+
+	/* Setup IRQs handler */
+	error = bus_setup_intr(dev, sc->res[0], INTR_TYPE_CLK,
+	    htif_intr, NULL, sc, &sc->ihl[0]);
+	if (error) {
+		device_printf(dev, "Unable to alloc int resource.\n");
+		return (ENXIO);
+	}
+
+	csr_set(sie, SIE_SSIE);
+
+	return (htif_enumerate(sc));
+}
+
+static device_method_t htif_methods[] = {
+	DEVMETHOD(device_probe,		htif_probe),
+	DEVMETHOD(device_attach,	htif_attach),
+
+	/* Bus interface */
+	DEVMETHOD(bus_read_ivar,	htif_read_ivar),
+
+	DEVMETHOD_END
+};
+
+static driver_t htif_driver = {
+	"htif",
+	htif_methods,
+	sizeof(struct htif_softc)
+};
+
+static devclass_t htif_devclass;
+
+DRIVER_MODULE(htif, simplebus, htif_driver,
+    htif_devclass, 0, 0);
diff --git a/sys/riscv/htif/htif.h b/sys/riscv/htif/htif.h
new file mode 100644
index 0000000..a1183d9
--- /dev/null
+++ b/sys/riscv/htif/htif.h
@@ -0,0 +1,93 @@
+/*-
+ * Copyright (c) 2015 Ruslan Bukin <br@bsdpad.com>
+ * All rights reserved.
+ *
+ * Portions of this software were developed by SRI International and the
+ * University of Cambridge Computer Laboratory under DARPA/AFRL contract
+ * FA8750-10-C-0237 ("CTSRD"), as part of the DARPA CRASH research programme.
+ *
+ * Portions of this software were developed by the University of Cambridge
+ * Computer Laboratory as part of the CTSRD Project, with support from the
+ * UK Higher Education Innovation Fund (HEIF).
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ */
+
+#define	HTIF_DEV_ID_SHIFT	(56)
+#define	HTIF_DEV_ID_MASK	(0xfful << HTIF_DEV_ID_SHIFT)
+#define	HTIF_CMD_SHIFT		(48)
+#define	HTIF_CMD_MASK		(0xfful << HTIF_CMD_SHIFT)
+#define	HTIF_DATA_SHIFT		(0)
+#define	HTIF_DATA_MASK		(0xffffffff << HTIF_DATA_SHIFT)
+
+#define	HTIF_CMD_READ			(0x00ul)
+#define	HTIF_CMD_WRITE			(0x01ul)
+#define	HTIF_CMD_READ_CONTROL_REG	(0x02ul)
+#define	HTIF_CMD_WRITE_CONTROL_REG	(0x03ul)
+#define	HTIF_CMD_IDENTIFY		(0xfful)
+#define	 IDENTIFY_PADDR_SHIFT		8
+#define	 IDENTIFY_IDENT			0xff
+
+#define	HTIF_NDEV		(256)
+#define	HTIF_ID_LEN		(64)
+#define	HTIF_ALIGN		(64)
+
+#define	HTIF_DEV_CMD(entry)	((entry & HTIF_CMD_MASK) >> HTIF_CMD_SHIFT)
+#define	HTIF_DEV_ID(entry)	((entry & HTIF_DEV_ID_MASK) >> HTIF_DEV_ID_SHIFT)
+#define	HTIF_DEV_DATA(entry)	((entry & HTIF_DATA_MASK) >> HTIF_DATA_SHIFT)
+
+/* bus softc */
+struct htif_softc {
+	struct resource		*res[1];
+	void			*ihl[1];
+	device_t		dev;
+	uint64_t		identify_id;
+	uint64_t		identify_done;
+};
+
+/* device private data */
+struct htif_dev_ivars {
+	char			*id;
+	int			index;
+	device_t		dev;
+	struct htif_softc	*sc;
+};
+
+uint64_t htif_command(uint64_t);
+int htif_setup_intr(int id, void *func, void *arg);
+int htif_read_ivar(device_t dev, device_t child, int which, uintptr_t *result);
+
+enum htif_device_ivars {
+	HTIF_IVAR_INDEX,
+	HTIF_IVAR_ID,
+};
+
+/*
+ * Simplified accessors for HTIF devices
+ */
+#define HTIF_ACCESSOR(var, ivar, type)	\
+	__BUS_ACCESSOR(htif, var, HTIF, ivar, type)
+
+HTIF_ACCESSOR(index, INDEX, int);
+HTIF_ACCESSOR(id, ID, char *);
diff --git a/sys/riscv/htif/htif_block.c b/sys/riscv/htif/htif_block.c
new file mode 100644
index 0000000..58804d7
--- /dev/null
+++ b/sys/riscv/htif/htif_block.c
@@ -0,0 +1,289 @@
+/*-
+ * Copyright (c) 2015 Ruslan Bukin <br@bsdpad.com>
+ * All rights reserved.
+ *
+ * Portions of this software were developed by SRI International and the
+ * University of Cambridge Computer Laboratory under DARPA/AFRL contract
+ * FA8750-10-C-0237 ("CTSRD"), as part of the DARPA CRASH research programme.
+ *
+ * Portions of this software were developed by the University of Cambridge
+ * Computer Laboratory as part of the CTSRD Project, with support from the
+ * UK Higher Education Innovation Fund (HEIF).
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/kernel.h>
+#include <sys/kthread.h>
+#include <sys/selinfo.h>
+#include <sys/module.h>
+#include <sys/lock.h>
+#include <sys/mutex.h>
+#include <sys/malloc.h>
+#include <sys/sysctl.h>
+#include <sys/uio.h>
+
+#include <sys/bio.h>
+#include <sys/bus.h>
+#include <sys/conf.h>
+#include <sys/disk.h>
+#include <geom/geom_disk.h>
+
+#include <vm/vm.h>
+#include <vm/pmap.h>
+
+#include <machine/md_var.h>
+#include <machine/bus.h>
+#include <machine/trap.h>
+#include <sys/rman.h>
+
+#include "htif.h"
+
+#define	SECTOR_SIZE_SHIFT	(9)
+#define	SECTOR_SIZE		(1 << SECTOR_SIZE_SHIFT)
+
+#define	HTIF_BLK_LOCK(_sc)	mtx_lock(&(_sc)->sc_mtx)
+#define	HTIF_BLK_UNLOCK(_sc)	mtx_unlock(&(_sc)->sc_mtx)
+#define	HTIF_BLK_LOCK_INIT(_sc) \
+	mtx_init(&_sc->sc_mtx, device_get_nameunit(_sc->dev), \
+	    "htif_blk", MTX_DEF)
+#define	HTIF_BLK_LOCK_DESTROY(_sc)	mtx_destroy(&_sc->sc_mtx);
+#define	HTIF_BLK_ASSERT_LOCKED(_sc)	mtx_assert(&_sc->sc_mtx, MA_OWNED);
+#define	HTIF_BLK_ASSERT_UNLOCKED(_sc)	mtx_assert(&_sc->sc_mtx, MA_NOTOWNED);
+
+static void htif_blk_task(void *arg);
+
+static disk_open_t	htif_blk_open;
+static disk_close_t	htif_blk_close;
+static disk_strategy_t	htif_blk_strategy;
+
+struct htif_blk_softc {
+	device_t	dev;
+	struct disk	*disk;
+	struct mtx	htif_io_mtx;
+	struct mtx	sc_mtx;
+	struct proc	*p;
+	struct bio_queue_head bio_queue;
+	int		running;
+	int		intr_chan;
+	int		cmd_done;
+	int		index;
+	uint16_t	curtag;
+};
+
+struct htif_blk_request {
+	uint64_t addr;
+	uint64_t offset;	/* offset in bytes */
+	uint64_t size;		/* length in bytes */
+	uint64_t tag;
+};
+
+static void
+htif_blk_intr(void *arg, uint64_t entry)
+{
+	struct htif_blk_softc *sc;
+	uint64_t devcmd;
+	uint64_t data;
+
+	sc = arg;
+
+	devcmd = HTIF_DEV_CMD(entry);
+	data = HTIF_DEV_DATA(entry);
+
+	if (sc->curtag == data) {
+		sc->cmd_done = 1;
+		wakeup(&sc->intr_chan);
+	}
+}
+
+static int
+htif_blk_probe(device_t dev)
+{
+
+	return (0);
+}
+
+static int
+htif_blk_attach(device_t dev)
+{
+	struct htif_blk_softc *sc;
+	char prefix[] = " size=";
+	char *str;
+	long size;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+
+	mtx_init(&sc->htif_io_mtx, device_get_nameunit(dev), "htif_blk", MTX_DEF);
+	HTIF_BLK_LOCK_INIT(sc);
+
+	str = strstr(htif_get_id(dev), prefix);
+
+	size = strtol((str + 6), NULL, 10);
+	if (size == 0) {
+		return (ENXIO);
+	}
+
+	sc->index = htif_get_index(dev);
+	if (sc->index < 0)
+		return (EINVAL);
+	htif_setup_intr(sc->index, htif_blk_intr, sc);
+
+	sc->disk = disk_alloc();
+	sc->disk->d_drv1 = sc;
+
+	sc->disk->d_maxsize = 4096; /* Max transfer */
+	sc->disk->d_name = "htif_blk";
+	sc->disk->d_open = htif_blk_open;
+	sc->disk->d_close = htif_blk_close;
+	sc->disk->d_strategy = htif_blk_strategy;
+	sc->disk->d_unit = 0;
+	sc->disk->d_sectorsize = SECTOR_SIZE;
+	sc->disk->d_mediasize = size;
+	disk_create(sc->disk, DISK_VERSION);
+
+	bioq_init(&sc->bio_queue);
+
+	sc->running = 1;
+
+	kproc_create(&htif_blk_task, sc, &sc->p, 0, 0, "%s: transfer", 
+	    device_get_nameunit(dev));
+
+	return (0);
+}
+
+static int
+htif_blk_open(struct disk *dp)
+{
+
+	return (0);
+}
+
+static int
+htif_blk_close(struct disk *dp)
+{
+
+	return (0);
+}
+
+static void
+htif_blk_task(void *arg)
+{
+	struct htif_blk_request req __aligned(HTIF_ALIGN);
+	struct htif_blk_softc *sc;
+	struct bio *bp;
+	uint64_t paddr;
+	uint64_t cmd;
+	int i;
+
+	sc = (struct htif_blk_softc *)arg;
+
+	while (1) {
+		HTIF_BLK_LOCK(sc);
+		do {
+			bp = bioq_takefirst(&sc->bio_queue);
+			if (bp == NULL)
+				msleep(sc, &sc->sc_mtx, PRIBIO, "jobqueue", 0);
+		} while (bp == NULL);
+		HTIF_BLK_UNLOCK(sc);
+
+		if (bp->bio_cmd == BIO_READ || bp->bio_cmd == BIO_WRITE) {
+			req.offset = (bp->bio_pblkno * sc->disk->d_sectorsize);
+			req.size = bp->bio_bcount;
+			paddr = vtophys(bp->bio_data);
+			KASSERT(paddr != 0, ("paddr is 0"));
+			req.addr = paddr;
+			req.tag = sc->curtag;
+
+			cmd = sc->index;
+			cmd <<= HTIF_DEV_ID_SHIFT;
+			if (bp->bio_cmd == BIO_READ)
+				cmd |= (HTIF_CMD_READ << HTIF_CMD_SHIFT);
+			else
+				cmd |= (HTIF_CMD_WRITE << HTIF_CMD_SHIFT);
+			paddr = vtophys(&req);
+			KASSERT(paddr != 0, ("paddr is 0"));
+			cmd |= paddr;
+
+			sc->cmd_done = 0;
+			htif_command(cmd);
+
+			/* Wait for interrupt */
+			HTIF_BLK_LOCK(sc);
+			i = 0;
+			while (sc->cmd_done == 0) {
+				msleep(&sc->intr_chan, &sc->sc_mtx, PRIBIO, "intr", hz/2);
+
+				if (i++ > 2) {
+					/* TODO: try to re-issue operation on timeout ? */
+					bp->bio_error = EIO;
+					bp->bio_flags |= BIO_ERROR;
+					disk_err(bp, "hard error", -1, 1);
+					break;
+				}
+			}
+			HTIF_BLK_UNLOCK(sc);
+
+			biodone(bp);
+		} else {
+			printf("unknown op %d\n", bp->bio_cmd);
+		}
+	}
+}
+
+static void
+htif_blk_strategy(struct bio *bp)
+{
+	struct htif_blk_softc *sc;
+
+	sc = bp->bio_disk->d_drv1;
+
+	HTIF_BLK_LOCK(sc);
+	if (sc->running > 0) {
+		bioq_disksort(&sc->bio_queue, bp);
+		HTIF_BLK_UNLOCK(sc);
+		wakeup(sc);
+	} else {
+		HTIF_BLK_UNLOCK(sc);
+		biofinish(bp, NULL, ENXIO);
+	}
+}
+
+static device_method_t htif_blk_methods[] = {
+	DEVMETHOD(device_probe,		htif_blk_probe),
+	DEVMETHOD(device_attach,	htif_blk_attach),
+};
+
+static driver_t htif_blk_driver = {
+	"htif_blk",
+	htif_blk_methods,
+	sizeof(struct htif_blk_softc)
+};
+
+static devclass_t	htif_blk_devclass;
+
+DRIVER_MODULE(htif_blk, htif, htif_blk_driver, htif_blk_devclass, 0, 0);
diff --git a/sys/riscv/htif/htif_console.c b/sys/riscv/htif/htif_console.c
new file mode 100644
index 0000000..b4a4676
--- /dev/null
+++ b/sys/riscv/htif/htif_console.c
@@ -0,0 +1,361 @@
+/*-
+ * Copyright (c) 2015 Ruslan Bukin <br@bsdpad.com>
+ * All rights reserved.
+ *
+ * Portions of this software were developed by SRI International and the
+ * University of Cambridge Computer Laboratory under DARPA/AFRL contract
+ * FA8750-10-C-0237 ("CTSRD"), as part of the DARPA CRASH research programme.
+ *
+ * Portions of this software were developed by the University of Cambridge
+ * Computer Laboratory as part of the CTSRD Project, with support from the
+ * UK Higher Education Innovation Fund (HEIF).
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/kdb.h>
+#include <sys/kernel.h>
+#include <sys/priv.h>
+#include <sys/systm.h>
+#include <sys/types.h>
+#include <sys/conf.h>
+#include <sys/cons.h>
+#include <sys/consio.h>
+#include <sys/tty.h>
+#include <sys/bus.h>
+#include <sys/module.h>
+
+#include <machine/bus.h>
+#include <machine/trap.h>
+
+#include "htif.h"
+
+#include <dev/ofw/openfirm.h>
+
+#include <ddb/ddb.h>
+
+extern uint64_t console_intr;
+
+static tsw_outwakeup_t riscvtty_outwakeup;
+
+static struct ttydevsw riscv_ttydevsw = {
+	.tsw_flags	= TF_NOPREFIX,
+	.tsw_outwakeup	= riscvtty_outwakeup,
+};
+
+static int			polltime;
+static struct callout		riscv_callout;
+static struct tty 		*tp = NULL;
+
+#if defined(KDB)
+static int			alt_break_state;
+#endif
+
+static void	riscv_timeout(void *);
+
+static cn_probe_t	riscv_cnprobe;
+static cn_init_t	riscv_cninit;
+static cn_term_t	riscv_cnterm;
+static cn_getc_t	riscv_cngetc;
+static cn_putc_t	riscv_cnputc;
+static cn_grab_t	riscv_cngrab;
+static cn_ungrab_t	riscv_cnungrab;
+
+CONSOLE_DRIVER(riscv);
+
+#define	MAX_BURST_LEN		1
+#define	QUEUE_SIZE		256
+#define	CONSOLE_DEFAULT_ID	1ul
+
+struct queue_entry {
+	uint64_t data;
+	uint64_t used;
+	struct queue_entry *next;
+};
+
+struct queue_entry cnqueue[QUEUE_SIZE];
+struct queue_entry *entry_last;
+struct queue_entry *entry_served;
+
+static void
+htif_putc(int c)
+{
+	uint64_t cmd;
+
+	cmd = (HTIF_CMD_WRITE << HTIF_CMD_SHIFT);
+	cmd |= (CONSOLE_DEFAULT_ID << HTIF_DEV_ID_SHIFT);
+	cmd |= c;
+
+	htif_command(cmd);
+}
+
+static uint8_t
+htif_getc(void)
+{
+	uint64_t cmd;
+	uint8_t res;
+
+	cmd = (HTIF_CMD_READ << HTIF_CMD_SHIFT);
+	cmd |= (CONSOLE_DEFAULT_ID << HTIF_DEV_ID_SHIFT);
+
+	res = htif_command(cmd);
+
+	return (res);
+}
+
+static void
+riscv_putc(int c)
+{
+	uint64_t counter;
+	uint64_t *cc;
+	uint64_t val;
+
+	val = 0;
+	counter = 0;
+
+	cc = (uint64_t*)&console_intr;
+	*cc = 0;
+
+	htif_putc(c);
+
+	/* Wait for an interrupt */
+	__asm __volatile(
+		"li	%0, 1\n"	/* counter = 1 */
+		"slli	%0, %0, 12\n"	/* counter <<= 12 */
+	"1:"
+		"addi	%0, %0, -1\n"	/* counter -= 1 */
+		"beqz	%0, 2f\n"	/* counter == 0 ? finish */
+		"ld	%1, 0(%2)\n"	/* val = *cc */
+		"beqz	%1, 1b\n"	/* val == 0 ? repeat */
+	"2:"
+		: "=&r"(counter), "=&r"(val) : "r"(cc)
+	);
+}
+
+#ifdef EARLY_PRINTF
+early_putc_t *early_putc = riscv_putc;
+#endif
+
+static void
+cn_drvinit(void *unused)
+{
+
+	if (riscv_consdev.cn_pri != CN_DEAD &&
+	    riscv_consdev.cn_name[0] != '\0') {
+		tp = tty_alloc(&riscv_ttydevsw, NULL);
+		tty_init_console(tp, 0);
+		tty_makedev(tp, NULL, "%s", "rcons");
+
+		polltime = 1;
+
+		callout_init(&riscv_callout, 1);
+		callout_reset(&riscv_callout, polltime, riscv_timeout, NULL);
+	}
+}
+
+SYSINIT(cndev, SI_SUB_CONFIGURE, SI_ORDER_MIDDLE, cn_drvinit, NULL);
+
+static void
+riscvtty_outwakeup(struct tty *tp)
+{
+	u_char buf[MAX_BURST_LEN];
+	int len;
+	int i;
+
+	for (;;) {
+		len = ttydisc_getc(tp, buf, sizeof(buf));
+		if (len == 0)
+			break;
+
+		KASSERT(len == 1, ("tty error"));
+
+		for (i = 0; i < len; i++)
+			riscv_putc(buf[i]);
+	}
+}
+
+static void
+riscv_timeout(void *v)
+{
+	int c;
+
+	tty_lock(tp);
+	while ((c = riscv_cngetc(NULL)) != -1)
+		ttydisc_rint(tp, c, 0);
+	ttydisc_rint_done(tp);
+	tty_unlock(tp);
+
+	callout_reset(&riscv_callout, polltime, riscv_timeout, NULL);
+}
+
+static void
+riscv_cnprobe(struct consdev *cp)
+{
+
+	cp->cn_pri = CN_NORMAL;
+}
+
+static void
+riscv_cninit(struct consdev *cp)
+{
+	int i;
+
+	strcpy(cp->cn_name, "rcons");
+
+	for (i = 0; i < QUEUE_SIZE; i++) {
+		if (i == (QUEUE_SIZE - 1))
+			cnqueue[i].next = &cnqueue[0];
+		else
+			cnqueue[i].next = &cnqueue[i+1];
+		cnqueue[i].data = 0;
+		cnqueue[i].used = 0;
+	}
+
+	entry_last = &cnqueue[0];
+	entry_served = &cnqueue[0];
+}
+
+static void
+riscv_cnterm(struct consdev *cp)
+{
+
+}
+
+static void
+riscv_cngrab(struct consdev *cp)
+{
+
+}
+
+static void
+riscv_cnungrab(struct consdev *cp)
+{
+
+}
+
+static int
+riscv_cngetc(struct consdev *cp)
+{
+	uint8_t data;
+	int ch;
+
+	ch = htif_getc();
+
+	if (entry_served->used == 1) {
+		data = entry_served->data;
+		entry_served->used = 0;
+		entry_served = entry_served->next;
+		ch = (data & 0xff);
+		if (ch > 0 && ch < 0xff) {
+#if defined(KDB)
+			kdb_alt_break(ch, &alt_break_state);
+#endif
+			return (ch);
+		}
+	}
+
+	return (-1);
+}
+
+static void
+riscv_cnputc(struct consdev *cp, int c)
+{
+
+	riscv_putc(c);
+}
+
+/*
+ * Bus interface.
+ */
+
+struct htif_console_softc {
+	device_t	dev;
+	int		running;
+	int		intr_chan;
+	int		cmd_done;
+	int		curtag;
+	int		index;
+};
+
+static void
+htif_console_intr(void *arg, uint64_t entry)
+{
+	struct htif_console_softc *sc;
+	uint8_t devcmd;
+	uint64_t data;
+
+	sc = arg;
+
+	devcmd = HTIF_DEV_CMD(entry);
+	data = HTIF_DEV_DATA(entry);
+
+	if (devcmd == 0) {
+		entry_last->data = data;
+		entry_last->used = 1;
+		entry_last = entry_last->next;
+	}
+}
+
+static int
+htif_console_probe(device_t dev)
+{
+
+	return (0);
+}
+
+static int
+htif_console_attach(device_t dev)
+{
+	struct htif_console_softc *sc;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+
+	sc->index = htif_get_index(dev);
+	if (sc->index < 0)
+		return (EINVAL);
+
+	htif_setup_intr(sc->index, htif_console_intr, sc);
+
+	return (0);
+}
+
+static device_method_t htif_console_methods[] = {
+	DEVMETHOD(device_probe,		htif_console_probe),
+	DEVMETHOD(device_attach,	htif_console_attach),
+	DEVMETHOD_END
+};
+
+static driver_t htif_console_driver = {
+	"htif_console",
+	htif_console_methods,
+	sizeof(struct htif_console_softc)
+};
+
+static devclass_t htif_console_devclass;
+
+DRIVER_MODULE(htif_console, htif, htif_console_driver,
+    htif_console_devclass, 0, 0);