riscv-memlayout: fix existing memlayout issues, add sbi interface

Existing memlayout code placed sections in overlapping areas, and would
overwrite the payload if it was large enough. Update memlayout.ld in
src/mainboard/emulation/spike-riscv to represent the spike emulator, and
add sbi interface which now has room into src/arch/riscv/bootblock.S.
Add utility code to qemu-riscv, but emulator itself has yet to be
updated to new ISA and as such should not be used.
Update Makefile to include all the files necessary for sbi interface.

Clean up unused include in src/arch/riscv/include/atomic.h and
whitespace in src/mainboard/emulation/spike-riscv/memlayout.ld
Fixed whitespace issues in spike_util.c

Change-Id: Id97fe75e45ac1361005bec6d421756ee3f98a508
Signed-off-by: Thaminda Edirisooriya <thaminda@google.com>
Reviewed-on: http://review.coreboot.org/11370
Tested-by: build bot (Jenkins)
Reviewed-by: Ronald G. Minnich <rminnich@gmail.com>

7 files changed, 454 insertions, 151 deletions

diff --git a/src/arch/riscv/Makefile.inc b/src/arch/riscv/Makefile.inc
index 6fac99c290..de6eb9183f 100644
--- a/src/arch/riscv/Makefile.inc
+++ b/src/arch/riscv/Makefile.inc
@@ -31,6 +31,7 @@ ifeq ($(CONFIG_ARCH_BOOTBLOCK_RISCV),y)
 bootblock-y = bootblock.S stages.c
 bootblock-y += trap_util.S
 bootblock-y += trap_handler.c
+bootblock-y += virtual_memory.c
 bootblock-y += boot.c
 bootblock-y += rom_media.c
 bootblock-y += \
@@ -85,6 +86,8 @@ endif
 ifeq ($(CONFIG_ARCH_RAMSTAGE_RISCV),y)
 
 ramstage-y =
+ramstage-y += trap_handler.c
+ramstage-y += virtual_memory.c
 ramstage-y += rom_media.c
 ramstage-y += stages.c
 ramstage-y += misc.c
diff --git a/src/arch/riscv/bootblock.S b/src/arch/riscv/bootblock.S
index a26b144458..4caeea6000 100644
--- a/src/arch/riscv/bootblock.S
+++ b/src/arch/riscv/bootblock.S
@@ -1,7 +1,7 @@
 /*
  * Early initialization code for aarch64 (a.k.a. armv8)
  *
- * Copyright 2013Google Inc.
+ * Copyright 2013 Google Inc.
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License as
@@ -22,34 +22,112 @@
 
 .section ".text._start", "ax", %progbits
 // Maybe there's a better way.
-.space 0x200
+# machine mode handler when in supervisor mode
+.space 0x140
+supervisor_machine_handler:
+  j supervisor_trap_entry
+
+# handler for when
+.space 0x7c
+.globl machine_handler
+machine_handler:
+#  call trap_handler
+  j trap_entry
+
+.space 0x3c
 .globl _start
 _start:
 
 	// pending figuring out this f-ing toolchain. Hardcode what we know works.
-//  la sp, 0x4ef0 // .stacktop
-//  la sp, 0x40000 // from src/mainboard/emulation/qemu-riscv
-  la sp, 0x7FF00 // stack start + stack size
+  la sp, 0x80FFF0 // stack start + stack size
 
-  // make room for HLS
+  # make room for HLS and initialize it
   addi sp, sp, -64 // MENTRY_FRAME_SIZE
-
+  csrr a0, mhartid
+  call hls_init
 
   //poison the stack
-  la t1, 0x40000
+  la t1, 0x800000
   li t0, 0xdeadbeef
   sd t0, 0(t1)
 
-//  la gp, _gp
+  la t0, exception_handler
+  csrw stvec, t0
 
   # clear any pending interrupts
-#if __GNUC__ < 5
-  csrwi clear_ipi, 0
-#else
   csrwi sip, 0
-#endif
 
+  # set up the mstatus register for VM
+  call mstatus_init
   call main
+.=0x2000
+    .space 0x800
+# sbi interface lives here
+
+# hart_id
+.align 5
+li a7, 0
+ecall
+ret
+
+# num_harts
+.align 4
+li a0, 1
+ret
+
+# query_memory
+.align 4
+li a7, 8
+ecall
+ret
+
+# console_putchar
+.align 4
+li a7, 1
+ecall
+ret
+
+# send_device_request
+.align 4
+li a7, 2
+ecall
+ret
+
+# receive_device_response
+.align 4
+li a7, 3
+ecall
+ret
+
+# send ipi
+.align 4
+li a7, 4
+ecall
+ret
+
+# clear ipi
+.align 4
+li a7, 5
+ecall
+ret
+
+# timebase
+.align 4
+li a0, 10000000 # temporary, we should provide the correct answer
+ret
+
+# shutdown
+.align 4
+li a7, 6
+ecall
+
+# set_timer
+.align 4
+li a7, 7
+ecall
+ret
+
+# end of SBI trampolines
 .=0x4000
 .stack:
 .align 8
@@ -59,7 +137,9 @@ _start:
 .align 3
 .stack_size:
 	.quad 0xf00
-
+.globl test_trap
+exception_handler:
+        call trap_handler
 reset:
 init_stack_loop:
 
diff --git a/src/arch/riscv/include/atomic.h b/src/arch/riscv/include/atomic.h
index 8d7295d930..f63f6e1fd9 100644
--- a/src/arch/riscv/include/atomic.h
+++ b/src/arch/riscv/include/atomic.h
@@ -3,7 +3,6 @@
 #ifndef _RISCV_ATOMIC_H
 #define _RISCV_ATOMIC_H
 
-//#include "config.h"
 #include <arch/encoding.h>
 
 #define disable_irqsave() clear_csr(sstatus, SSTATUS_IE)
diff --git a/src/mainboard/emulation/qemu-riscv/Makefile.inc b/src/mainboard/emulation/qemu-riscv/Makefile.inc
index e60e0c1051..87bc39ad2e 100644
--- a/src/mainboard/emulation/qemu-riscv/Makefile.inc
+++ b/src/mainboard/emulation/qemu-riscv/Makefile.inc
@@ -14,9 +14,12 @@
 
 bootblock-y += bootblock.c
 bootblock-y += uart.c
+bootblock-y += qemu_util.c
 romstage-y += romstage.c
+romstage-y += qemu_util.c
 romstage-y += uart.c
 ramstage-y += uart.c
+ramstage-y += qemu_util.c
 
 bootblock-y += memlayout.ld
 romstage-y += memlayout.ld
diff --git a/src/mainboard/emulation/qemu-riscv/qemu_util.c b/src/mainboard/emulation/qemu-riscv/qemu_util.c
new file mode 100644
index 0000000000..fca7d56ed0
--- /dev/null
+++ b/src/mainboard/emulation/qemu-riscv/qemu_util.c
@@ -0,0 +1,218 @@
+/*
+ * Copyright (c) 2013, The Regents of the University of California (Regents).
+ * All Rights Reserved.
+ *
+ * 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.
+ * 3. Neither the name of the Regents nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT,
+ * SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING
+ * OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS
+ * BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED
+ * HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE
+ * MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
+ */
+
+#include <spike_util.h>
+#include <arch/errno.h>
+#include <atomic.h>
+#include <string.h>
+#include <console/console.h>
+
+uintptr_t translate_address(uintptr_t vAddr) {
+	// TODO: implement the page table translation algorithm
+	//uintptr_t pageTableRoot = read_csr(sptbr);
+	uintptr_t physAddrMask = 0xfffffff;
+	uintptr_t translationResult = vAddr & physAddrMask;
+	printk(BIOS_DEBUG, "Translated virtual address 0x%llx to physical address 0x%llx\n", vAddr, translationResult);
+	return translationResult;
+}
+
+uintptr_t mcall_query_memory(uintptr_t id, memory_block_info *p)
+{
+	uintptr_t physicalAddr = translate_address((uintptr_t) p);
+	memory_block_info *info = (memory_block_info*) physicalAddr;
+	if (id == 0) {
+		info->base = 0x1000000; // hard coded for now, but we can put these values somewhere later
+		info->size = 0x7F000000 - info->base;
+		return 0;
+	}
+
+	return -1;
+}
+
+uintptr_t mcall_send_ipi(uintptr_t recipient)
+{
+	//if (recipient >= num_harts)
+	//return -1;
+
+	if (atomic_swap(&OTHER_HLS(recipient)->ipi_pending, 1) == 0) {
+		mb();
+		write_csr(send_ipi, recipient);
+	}
+
+	return 0;
+}
+
+uintptr_t mcall_clear_ipi(void)
+{
+	// only clear SSIP if no other events are pending
+	if (HLS()->device_response_queue_head == NULL) {
+		clear_csr(mip, MIP_SSIP);
+		mb();
+	}
+
+	return atomic_swap(&HLS()->ipi_pending, 0);
+}
+
+uintptr_t mcall_shutdown(void)
+{
+	while (1) write_csr(mtohost, 1);
+	return 0;
+}
+
+uintptr_t mcall_set_timer(unsigned long long when)
+{
+	write_csr(mtimecmp, when);
+	clear_csr(mip, MIP_STIP);
+	set_csr(mie, MIP_MTIP);
+	return 0;
+}
+
+uintptr_t mcall_dev_req(sbi_device_message *m)
+{
+	if ((m->dev > 0xFFU) | (m->cmd > 0xFFU) | (m->data > 0x0000FFFFFFFFFFFFU)) return -EINVAL;
+
+	while (swap_csr(mtohost, TOHOST_CMD(m->dev, m->cmd, m->data)) != 0);
+
+	m->sbi_private_data = (uintptr_t)HLS()->device_request_queue_head;
+	HLS()->device_request_queue_head = m;
+	HLS()->device_request_queue_size++;
+
+	return 0;
+}
+
+uintptr_t mcall_dev_resp(void)
+{
+	htif_interrupt(0, 0);
+
+	sbi_device_message* m = HLS()->device_response_queue_head;
+	if (m) {
+		//printm("resp %p\n", m);
+		sbi_device_message* next = (void*)atomic_read(&m->sbi_private_data);
+		HLS()->device_response_queue_head = next;
+		if (!next) {
+			HLS()->device_response_queue_tail = 0;
+
+			// only clear SSIP if no other events are pending
+			clear_csr(mip, MIP_SSIP);
+			mb();
+			if (HLS()->ipi_pending) set_csr(mip, MIP_SSIP);
+		}
+	}
+	return (uintptr_t)m;
+}
+
+uintptr_t mcall_hart_id(void)
+{
+	return HLS()->hart_id;
+}
+
+void hls_init(uint32_t hart_id)
+{
+	memset(HLS(), 0, sizeof(*HLS()));
+	HLS()->hart_id = hart_id;
+}
+
+uintptr_t htif_interrupt(uintptr_t mcause, uintptr_t* regs) {
+	uintptr_t fromhost = swap_csr(mfromhost, 0);
+	if (!fromhost)
+	return 0;
+
+	uintptr_t dev = FROMHOST_DEV(fromhost);
+	uintptr_t cmd = FROMHOST_CMD(fromhost);
+	uintptr_t data = FROMHOST_DATA(fromhost);
+
+	sbi_device_message* m = HLS()->device_request_queue_head;
+	sbi_device_message* prev = 0x0;
+	unsigned long i, n;
+	for (i = 0, n = HLS()->device_request_queue_size; i < n; i++) {
+		/*
+		if (!supervisor_paddr_valid(m, sizeof(*m))
+		&& EXTRACT_FIELD(read_csr(mstatus), MSTATUS_PRV1) != PRV_M)
+		panic("htif: page fault");
+		*/
+
+		sbi_device_message* next = (void*)m->sbi_private_data;
+		if (m->dev == dev && m->cmd == cmd) {
+			m->data = data;
+
+			// dequeue from request queue
+			if (prev)
+			prev->sbi_private_data = (uintptr_t)next;
+			else
+			HLS()->device_request_queue_head = next;
+			HLS()->device_request_queue_size = n-1;
+			m->sbi_private_data = 0;
+
+			// enqueue to response queue
+			if (HLS()->device_response_queue_tail)
+			{
+				HLS()->device_response_queue_tail->sbi_private_data = (uintptr_t)m;
+			}
+			else
+			{
+				HLS()->device_response_queue_head = m;
+			}
+			HLS()->device_response_queue_tail = m;
+
+			// signal software interrupt
+			set_csr(mip, MIP_SSIP);
+			return 0;
+		}
+
+		prev = m;
+		m = (void*)atomic_read(&m->sbi_private_data);
+	}
+	//HLT();
+	return 0;
+	//panic("htif: no record");
+}
+
+uintptr_t mcall_console_putchar(uint8_t ch)
+{
+	while (swap_csr(mtohost, TOHOST_CMD(1, 1, ch)) != 0);
+	while (1) {
+		uintptr_t fromhost = read_csr(mfromhost);
+		if (FROMHOST_DEV(fromhost) != 1 || FROMHOST_CMD(fromhost) != 1) {
+		if (fromhost)
+		htif_interrupt(0, 0);
+		continue;
+	}
+	write_csr(mfromhost, 0);
+	break;
+	}
+	return 0;
+}
+
+void testPrint(void) {
+	/* Print a test command to check Spike console output */
+	mcall_console_putchar('h');
+	mcall_console_putchar('e');
+	mcall_console_putchar('l');
+	mcall_console_putchar('l');
+	mcall_console_putchar('o');
+	mcall_console_putchar('\n');
+}
diff --git a/src/mainboard/emulation/spike-riscv/memlayout.ld b/src/mainboard/emulation/spike-riscv/memlayout.ld
index 8801f3520b..8e2e7eedf7 100644
--- a/src/mainboard/emulation/spike-riscv/memlayout.ld
+++ b/src/mainboard/emulation/spike-riscv/memlayout.ld
@@ -23,10 +23,10 @@
 
 SECTIONS
 {
-	DRAM_START(0x0)
+        DRAM_START(0x0)
 	BOOTBLOCK(0x0, 64K)
-	ROMSTAGE(0x20000, 128K)
-	STACK(0x40000, 0x3ff00)
-	PRERAM_CBMEM_CONSOLE(0x80000, 8K)
-	RAMSTAGE(0x100000, 16M)
+        STACK(8M, 64K)
+	ROMSTAGE(8M + 64K, 128K)
+	PRERAM_CBMEM_CONSOLE(8M + 192k, 8K)
+	RAMSTAGE(8M + 200K, 256K)
 }
diff --git a/src/mainboard/emulation/spike-riscv/spike_util.c b/src/mainboard/emulation/spike-riscv/spike_util.c
index 9edc62da2b..fca7d56ed0 100644
--- a/src/mainboard/emulation/spike-riscv/spike_util.c
+++ b/src/mainboard/emulation/spike-riscv/spike_util.c
@@ -32,187 +32,187 @@
 #include <console/console.h>
 
 uintptr_t translate_address(uintptr_t vAddr) {
-        // TODO: implement the page table translation algorithm
-        //uintptr_t pageTableRoot = read_csr(sptbr);
-        uintptr_t physAddrMask = 0xfffffff;
-        uintptr_t translationResult = vAddr & physAddrMask;
-        printk(BIOS_DEBUG, "Translated virtual address 0x%llx to physical address 0x%llx\n", vAddr, translationResult);
-        return translationResult;
+	// TODO: implement the page table translation algorithm
+	//uintptr_t pageTableRoot = read_csr(sptbr);
+	uintptr_t physAddrMask = 0xfffffff;
+	uintptr_t translationResult = vAddr & physAddrMask;
+	printk(BIOS_DEBUG, "Translated virtual address 0x%llx to physical address 0x%llx\n", vAddr, translationResult);
+	return translationResult;
 }
 
 uintptr_t mcall_query_memory(uintptr_t id, memory_block_info *p)
 {
-        uintptr_t physicalAddr = translate_address((uintptr_t) p);
-        memory_block_info *info = (memory_block_info*) physicalAddr;
-        if (id == 0) {
-                info->base = 0x1000000; // hard coded for now, but we can put these values somewhere later
-                info->size = 0x7F000000 - info->base;
-                return 0;
-        }
-
-        return -1;
+	uintptr_t physicalAddr = translate_address((uintptr_t) p);
+	memory_block_info *info = (memory_block_info*) physicalAddr;
+	if (id == 0) {
+		info->base = 0x1000000; // hard coded for now, but we can put these values somewhere later
+		info->size = 0x7F000000 - info->base;
+		return 0;
+	}
+
+	return -1;
 }
 
 uintptr_t mcall_send_ipi(uintptr_t recipient)
 {
-        //if (recipient >= num_harts)
-        //return -1;
+	//if (recipient >= num_harts)
+	//return -1;
 
-        if (atomic_swap(&OTHER_HLS(recipient)->ipi_pending, 1) == 0) {
-                mb();
-                write_csr(send_ipi, recipient);
-        }
+	if (atomic_swap(&OTHER_HLS(recipient)->ipi_pending, 1) == 0) {
+		mb();
+		write_csr(send_ipi, recipient);
+	}
 
-        return 0;
+	return 0;
 }
 
 uintptr_t mcall_clear_ipi(void)
 {
-        // only clear SSIP if no other events are pending
-        if (HLS()->device_response_queue_head == NULL) {
-                clear_csr(mip, MIP_SSIP);
-                mb();
-        }
+	// only clear SSIP if no other events are pending
+	if (HLS()->device_response_queue_head == NULL) {
+		clear_csr(mip, MIP_SSIP);
+		mb();
+	}
 
-        return atomic_swap(&HLS()->ipi_pending, 0);
+	return atomic_swap(&HLS()->ipi_pending, 0);
 }
 
 uintptr_t mcall_shutdown(void)
 {
-        while (1) write_csr(mtohost, 1);
-        return 0;
+	while (1) write_csr(mtohost, 1);
+	return 0;
 }
 
 uintptr_t mcall_set_timer(unsigned long long when)
 {
-        write_csr(mtimecmp, when);
-        clear_csr(mip, MIP_STIP);
-        set_csr(mie, MIP_MTIP);
-        return 0;
+	write_csr(mtimecmp, when);
+	clear_csr(mip, MIP_STIP);
+	set_csr(mie, MIP_MTIP);
+	return 0;
 }
 
 uintptr_t mcall_dev_req(sbi_device_message *m)
 {
-        if ((m->dev > 0xFFU) | (m->cmd > 0xFFU) | (m->data > 0x0000FFFFFFFFFFFFU)) return -EINVAL;
+	if ((m->dev > 0xFFU) | (m->cmd > 0xFFU) | (m->data > 0x0000FFFFFFFFFFFFU)) return -EINVAL;
 
-        while (swap_csr(mtohost, TOHOST_CMD(m->dev, m->cmd, m->data)) != 0);
+	while (swap_csr(mtohost, TOHOST_CMD(m->dev, m->cmd, m->data)) != 0);
 
-        m->sbi_private_data = (uintptr_t)HLS()->device_request_queue_head;
-        HLS()->device_request_queue_head = m;
-        HLS()->device_request_queue_size++;
+	m->sbi_private_data = (uintptr_t)HLS()->device_request_queue_head;
+	HLS()->device_request_queue_head = m;
+	HLS()->device_request_queue_size++;
 
-        return 0;
+	return 0;
 }
 
 uintptr_t mcall_dev_resp(void)
 {
-        htif_interrupt(0, 0);
-
-        sbi_device_message* m = HLS()->device_response_queue_head;
-        if (m) {
-                //printm("resp %p\n", m);
-                sbi_device_message* next = (void*)atomic_read(&m->sbi_private_data);
-                HLS()->device_response_queue_head = next;
-                if (!next) {
-                        HLS()->device_response_queue_tail = 0;
-
-                        // only clear SSIP if no other events are pending
-                        clear_csr(mip, MIP_SSIP);
-                        mb();
-                        if (HLS()->ipi_pending) set_csr(mip, MIP_SSIP);
-                }
-        }
-        return (uintptr_t)m;
+	htif_interrupt(0, 0);
+
+	sbi_device_message* m = HLS()->device_response_queue_head;
+	if (m) {
+		//printm("resp %p\n", m);
+		sbi_device_message* next = (void*)atomic_read(&m->sbi_private_data);
+		HLS()->device_response_queue_head = next;
+		if (!next) {
+			HLS()->device_response_queue_tail = 0;
+
+			// only clear SSIP if no other events are pending
+			clear_csr(mip, MIP_SSIP);
+			mb();
+			if (HLS()->ipi_pending) set_csr(mip, MIP_SSIP);
+		}
+	}
+	return (uintptr_t)m;
 }
 
 uintptr_t mcall_hart_id(void)
 {
-        return HLS()->hart_id;
+	return HLS()->hart_id;
 }
 
 void hls_init(uint32_t hart_id)
 {
-        memset(HLS(), 0, sizeof(*HLS()));
-        HLS()->hart_id = hart_id;
+	memset(HLS(), 0, sizeof(*HLS()));
+	HLS()->hart_id = hart_id;
 }
 
 uintptr_t htif_interrupt(uintptr_t mcause, uintptr_t* regs) {
-        uintptr_t fromhost = swap_csr(mfromhost, 0);
-        if (!fromhost)
-        return 0;
-
-        uintptr_t dev = FROMHOST_DEV(fromhost);
-        uintptr_t cmd = FROMHOST_CMD(fromhost);
-        uintptr_t data = FROMHOST_DATA(fromhost);
-
-        sbi_device_message* m = HLS()->device_request_queue_head;
-        sbi_device_message* prev = 0x0;
-        unsigned long i, n;
-        for (i = 0, n = HLS()->device_request_queue_size; i < n; i++) {
-                /*
-                if (!supervisor_paddr_valid(m, sizeof(*m))
-                && EXTRACT_FIELD(read_csr(mstatus), MSTATUS_PRV1) != PRV_M)
-                panic("htif: page fault");
-                */
-
-                sbi_device_message* next = (void*)m->sbi_private_data;
-                if (m->dev == dev && m->cmd == cmd) {
-                        m->data = data;
-
-                        // dequeue from request queue
-                        if (prev)
-                        prev->sbi_private_data = (uintptr_t)next;
-                        else
-                        HLS()->device_request_queue_head = next;
-                        HLS()->device_request_queue_size = n-1;
-                        m->sbi_private_data = 0;
-
-                        // enqueue to response queue
-                        if (HLS()->device_response_queue_tail)
-                        {
-                                HLS()->device_response_queue_tail->sbi_private_data = (uintptr_t)m;
-                        }
-                        else
-                        {
-                                HLS()->device_response_queue_head = m;
-                        }
-                        HLS()->device_response_queue_tail = m;
-
-                        // signal software interrupt
-                        set_csr(mip, MIP_SSIP);
-                        return 0;
-                }
-
-                prev = m;
-                m = (void*)atomic_read(&m->sbi_private_data);
-        }
-        //HLT();
-        return 0;
-        //panic("htif: no record");
+	uintptr_t fromhost = swap_csr(mfromhost, 0);
+	if (!fromhost)
+	return 0;
+
+	uintptr_t dev = FROMHOST_DEV(fromhost);
+	uintptr_t cmd = FROMHOST_CMD(fromhost);
+	uintptr_t data = FROMHOST_DATA(fromhost);
+
+	sbi_device_message* m = HLS()->device_request_queue_head;
+	sbi_device_message* prev = 0x0;
+	unsigned long i, n;
+	for (i = 0, n = HLS()->device_request_queue_size; i < n; i++) {
+		/*
+		if (!supervisor_paddr_valid(m, sizeof(*m))
+		&& EXTRACT_FIELD(read_csr(mstatus), MSTATUS_PRV1) != PRV_M)
+		panic("htif: page fault");
+		*/
+
+		sbi_device_message* next = (void*)m->sbi_private_data;
+		if (m->dev == dev && m->cmd == cmd) {
+			m->data = data;
+
+			// dequeue from request queue
+			if (prev)
+			prev->sbi_private_data = (uintptr_t)next;
+			else
+			HLS()->device_request_queue_head = next;
+			HLS()->device_request_queue_size = n-1;
+			m->sbi_private_data = 0;
+
+			// enqueue to response queue
+			if (HLS()->device_response_queue_tail)
+			{
+				HLS()->device_response_queue_tail->sbi_private_data = (uintptr_t)m;
+			}
+			else
+			{
+				HLS()->device_response_queue_head = m;
+			}
+			HLS()->device_response_queue_tail = m;
+
+			// signal software interrupt
+			set_csr(mip, MIP_SSIP);
+			return 0;
+		}
+
+		prev = m;
+		m = (void*)atomic_read(&m->sbi_private_data);
+	}
+	//HLT();
+	return 0;
+	//panic("htif: no record");
 }
 
 uintptr_t mcall_console_putchar(uint8_t ch)
 {
-        while (swap_csr(mtohost, TOHOST_CMD(1, 1, ch)) != 0);
-        while (1) {
-                uintptr_t fromhost = read_csr(mfromhost);
-                if (FROMHOST_DEV(fromhost) != 1 || FROMHOST_CMD(fromhost) != 1) {
-                if (fromhost)
-                htif_interrupt(0, 0);
-                continue;
-        }
-        write_csr(mfromhost, 0);
-        break;
-        }
-        return 0;
+	while (swap_csr(mtohost, TOHOST_CMD(1, 1, ch)) != 0);
+	while (1) {
+		uintptr_t fromhost = read_csr(mfromhost);
+		if (FROMHOST_DEV(fromhost) != 1 || FROMHOST_CMD(fromhost) != 1) {
+		if (fromhost)
+		htif_interrupt(0, 0);
+		continue;
+	}
+	write_csr(mfromhost, 0);
+	break;
+	}
+	return 0;
 }
 
 void testPrint(void) {
-        /* Print a test command to check Spike console output */
-        mcall_console_putchar('h');
-        mcall_console_putchar('e');
-        mcall_console_putchar('l');
-        mcall_console_putchar('l');
-        mcall_console_putchar('o');
-        mcall_console_putchar('\n');
+	/* Print a test command to check Spike console output */
+	mcall_console_putchar('h');
+	mcall_console_putchar('e');
+	mcall_console_putchar('l');
+	mcall_console_putchar('l');
+	mcall_console_putchar('o');
+	mcall_console_putchar('\n');
 }