#include <types.h>
#include <kmsg.h>
#include <errno.h>
#include <mman.h>
#include <kmalloc.h>
#include <cls.h>
#include <syscall.h>
#include <memory.h>
#include <process.h>
#include <mc_perf_event.h>
#include <bootparam.h>
#include <kref.h>
#include <io.h>

#include <tofu/tof_uapi.h>
#include <tofu/tof_icc.h>

/* DWARF generated headers */
#include <tofu/tofu_generated-tof_core_cq.h>
#include <tofu/tofu_generated-tof_utofu_device.h>
#include <tofu/tofu_generated-tof_utofu_cq.h>
#include <tofu/tofu_generated-tof_utofu_mbpt.h>

#define TOF_UTOFU_VERSION TOF_UAPI_VERSION
#define TOF_UTOFU_NUM_STAG_NTYPES 3
#define TOF_UTOFU_NUM_STAG_BITS(size) ((size) + 13)
#define TOF_UTOFU_NUM_STAG(size) ((uint64_t)1 << TOF_UTOFU_NUM_STAG_BITS(size))
#define TOF_UTOFU_STAG_TRANS_BITS 3
#define TOF_UTOFU_STAG_TRANS_SIZE ((uint64_t)1 << TOF_UTOFU_STAG_TRANS_BITS)
#define TOF_UTOFU_STAG_TRANS_TABLE_LEN(size) (TOF_UTOFU_NUM_STAG(size) * TOF_UTOFU_STAG_TRANS_SIZE)
#define TOF_UTOFU_STEERING_TABLE_LEN(size) (TOF_UTOFU_NUM_STAG(size) * TOF_ICC_STEERING_SIZE)
#define TOF_UTOFU_MB_TABLE_LEN(size) (TOF_UTOFU_NUM_STAG(size) * TOF_ICC_MB_SIZE)
#define TOF_UTOFU_STAG_MEM_LEN(size) (TOF_UTOFU_STEERING_TABLE_LEN(size) * 4)
#define TOF_UTOFU_SPECIAL_STAG 4096

#define TOF_UTOFU_ICC_COMMON_REGISTER (tof_icc_reg_pa + 0x0B000000)
#define TOF_UTOFU_REG_START tof_icc_reg_pa
#define TOF_UTOFU_REG_END (TOF_UTOFU_ICC_COMMON_REGISTER + 0x000FFFFF)

#define TOF_UTOFU_SET_SUBNET_TNI	0	/* This number is kernel TNIs number in setting subnet */
#define TOF_UTOFU_KCQ			11
#define TOF_UTOFU_LINKDOWN_PORT_MASK	0x000003FF

#define TOF_UTOFU_ALLOC_STAG_LPG	0x2
#define TOF_UTOFU_BLANK_MBVA (-1)

#define TOF_UTOFU_MRU_EMPTY (-1)

/* The `const' in roundup() prevents gcc-3.3 from calling __divdi3 */
#define roundup(x, y) (					\
{							\
	const typeof(y) __y = y;			\
	(((x) + (__y - 1)) / __y) * __y;		\
}							\
)
#define rounddown(x, y) (				\
{							\
	typeof(x) __x = (x);				\
	__x - (__x % (y));				\
}							\
)

struct tof_utofu_trans_list {
	int16_t prev;
	int16_t next;
	uint8_t pgszbits;
	struct tof_utofu_mbpt *mbpt;
};

static inline uintptr_t tof_utofu_get_stag_start(struct tof_utofu_cq *ucq, int stag)
{
	return ((uintptr_t)ucq->trans.table[stag].steering.bits.start) << PAGE_SHIFT;
}
static inline size_t tof_utofu_get_stag_len(struct tof_utofu_cq *ucq, int stag)
{
	return ((size_t)ucq->trans.table[stag].steering.bits.len) << PAGE_SHIFT;
}
static inline uintptr_t tof_utofu_get_mbpt_start(struct tof_utofu_cq *ucq, int stag)
{
	return ((uintptr_t)ucq->trans.table[stag].mbpt.bits.start) << PAGE_SHIFT;
}
static inline size_t tof_utofu_get_mbpt_len(struct tof_utofu_cq *ucq, int stag)
{
	return ((size_t)ucq->trans.table[stag].mbpt.bits.len) << PAGE_SHIFT;
}

#define raw_rc_output(fmt, args...) kprintf("%s: ", fmt, __func__, ##args)
static int tof_utofu_raw_rc_output_supress = 1;
static int tof_utofu_mbpt_address_match = 1;

static int tof_utofu_get_pagesize_locked(uintptr_t addr, size_t len,
		uint8_t *_pgszbits, bool readonly) {
	uint8_t cur_shift;
	uint8_t min_shift = U8_MAX;
	uintptr_t start, end, va;
	struct process *proc = cpu_local_var(current)->proc;
	struct process_vm *vm = cpu_local_var(current)->vm;
	int p2align;
	size_t psize;
	pte_t *ptep;
	//struct vm_range *range;

	if(addr < PAGE_SIZE){
		*_pgszbits = PAGE_SHIFT;
		return 0;
	}

	start = round_down(addr, PAGE_SIZE);
	end = round_up(addr + len, PAGE_SIZE);

	//range = lookup_process_memory_range(vm, start, end);
	//if (!range) {
	//	return -EFAULT;
	//}

	/* Special case for straight mapping */
	if (proc->straight_va && (void *)start >= proc->straight_va &&
		(void *)end < proc->straight_va + proc->straight_len) {

		if (end - start < PTL2_SIZE) {
			*_pgszbits = PTL1_SHIFT;
		}
		else {
			*_pgszbits = PTL2_SHIFT;
		}
		return 0;
	}

	for (va = start; va < end; va += ((size_t)1 << cur_shift)) {
		ptep = ihk_mc_pt_lookup_fault_pte(vm, (void *)va,
				0, NULL, &psize, &p2align);

		if (unlikely(!ptep || !pte_is_present(ptep))) {
			kprintf("%s: ERROR: no valid PTE for 0x%lx\n",
					__func__, va);
			return -EFAULT;
		}

		cur_shift = p2align + PAGE_SHIFT;

		if (cur_shift < min_shift) {
			min_shift = cur_shift;
		}

		if (min_shift <= PAGE_SHIFT) {
			break;
		}
	}

#if 0
	/* Tofu only support 64kB and 2MB pages */
	if (min_shift > PTL1_CONT_SHIFT)
		min_shift = PTL1_CONT_SHIFT;
#endif

	*_pgszbits = min_shift;
	return 0;
}

static int tof_utofu_trans_search(struct tof_utofu_cq *ucq, uintptr_t start, uintptr_t end, uint8_t pgszbits, bool readonly){
	struct tof_utofu_trans_list *mru = ucq->trans.mru;
	uintptr_t stagstart, stagend;
	int stag;

	stag = ucq->trans.mruhead;
	if(stag == TOF_UTOFU_MRU_EMPTY){
		if(unlikely(!tof_utofu_raw_rc_output_supress)){
			raw_rc_output(-ENOENT);
		}
		return -ENOENT;
	}
	do {
		stagstart = tof_utofu_get_stag_start(ucq, stag);
		stagend = stagstart + tof_utofu_get_stag_len(ucq, stag);
		if(stag >= TOF_UTOFU_SPECIAL_STAG && ((stag & 0x1) == readonly) && (mru[stag].pgszbits == pgszbits)) {
			if ((tof_utofu_mbpt_address_match & 0x1)) {
				if ((stagstart == start) && (stagend == end)) {
				kprintf("%s: found stag: %d\n", __func__, stag);
					return stag;
				}
			}
			else {
				if ((stagstart <= start) && (end <= stagend)) {
					return stag;
				}
			}
		}
		stag = ucq->trans.mru[stag].next;
	} while(stag != ucq->trans.mruhead);
	if(unlikely(!tof_utofu_raw_rc_output_supress)){
		raw_rc_output(-ENOENT);
	}
	dkprintf("%s: -ENOENT\n", __func__);
	return -ENOENT;
}

static int tof_utofu_reserve_stag(struct tof_utofu_cq *ucq, bool readonly){
	int stag;
	for(stag = TOF_UTOFU_SPECIAL_STAG + readonly; stag < TOF_UTOFU_NUM_STAG(ucq->num_stag); stag += 2){
		if(!ucq->steering[stag].enable){
			dkprintf("%s: could use: %d\n", __func__, stag);
			return stag;
		}
	}
	return -1;
}

static int tof_utofu_calc_mbptstart(int64_t start, int64_t end, size_t mbpt_npages, uint8_t pgszbits,
	uintptr_t *mbptstart)
{
#if 0
	struct vm_area_struct *vma;
	int64_t len = mbpt_npages << pgszbits;
	size_t pgsz = (size_t)1 << pgszbits;

	vma = find_vma(current->mm, start);
	if(vma == NULL || vma->vm_start > start || vma->vm_end < end){
		return -ENOENT;
	}

	if(vma->vm_flags & VM_GROWSDOWN){
		/* stack */
		/* we cannot extend MBPTs to lower address.
		 * therefore, we allocate rather large MBPT. */
		int64_t upperbound;
		uintptr_t mbpttail;
		upperbound = round_up(vma->vm_end, pgsz);
		if ((start + len) < 0) {
			mbpttail = upperbound;
		}
		else {
			mbpttail = min(upperbound, start + len);
		}
		*mbptstart = mbpttail - len;
	}else{
		int64_t lowerbound;
		lowerbound = round_down(vma->vm_start, pgsz);
		*mbptstart = max(lowerbound, end - len);
	}
#else
	*mbptstart = start;
#endif

	return 0;
}

int16_t *tof_ib_stag_list;
ihk_spinlock_t *tof_ib_stag_lock;
int *tof_ib_stag_list_Rp_addr;
#define tof_ib_stag_list_Rp (*tof_ib_stag_list_Rp_addr)
int *tof_ib_stag_list_Wp_addr;
#define tof_ib_stag_list_Wp (*tof_ib_stag_list_Wp_addr)
#define TOF_IB_MAX_STAG 0x4000

static int16_t tof_ib_stag_alloc(void){
	int16_t ret;
	unsigned long flags;

	linux_spin_lock_irqsave(tof_ib_stag_lock, flags);

	if(tof_ib_stag_list_Rp != tof_ib_stag_list_Wp){
		ret = tof_ib_stag_list[tof_ib_stag_list_Rp];
		tof_ib_stag_list_Rp = (tof_ib_stag_list_Rp + 1) % TOF_IB_MAX_STAG;
	}
	else{
		/* empty */
		ret =  -ENOENT;
	}
	linux_spin_unlock_irqrestore(tof_ib_stag_lock, flags);

	dkprintf("%s: stag: %d allocated\n", __func__, ret);
	return ret;
}

static void tof_ib_stag_free(int16_t stag){
	int16_t next;
	unsigned long flags;

	linux_spin_lock_irqsave(tof_ib_stag_lock, flags);

	next = (tof_ib_stag_list_Wp + 1) % TOF_IB_MAX_STAG;
	if(next != tof_ib_stag_list_Rp){ /* next == tof_ib_stag_list_Rp is full. */
		tof_ib_stag_list[tof_ib_stag_list_Wp] = stag;
		tof_ib_stag_list_Wp = next;
	}
	linux_spin_unlock_irqrestore(tof_ib_stag_lock, flags);

	dkprintf("%s: stag: %d freed\n", __func__, stag);
}

struct tof_util_aligned_mem {
	void *mem;
	int nr_pages;
	uint32_t offset;  /* should be less than PAGE_SIZE */
};
static struct tof_util_aligned_mem *tof_ib_mbpt_mem = NULL;
static struct tof_icc_steering_entry *tof_ib_steering = NULL;
static struct tof_icc_mb_entry *tof_ib_mb = NULL;

static int tof_ib_steering_enable(int stag, uint64_t mbpt_ipa, size_t npages, size_t length, uint64_t mbva){
	struct tof_icc_steering_entry *steering = &tof_ib_steering[stag];
	struct tof_icc_mb_entry *mb = &tof_ib_mb[stag];
	if(steering->enable != 0 || mb->enable != 0){
		return -EBUSY;
	}
	mb->ps = TOF_ICC_MB_PS_ENCODE(PAGE_SHIFT);  /* will be 0 */
	mb->enable = 1;
	mb->ipa = mbpt_ipa >> 8;
	mb->npage = npages;
	steering->readonly = 0;
	steering->mbid = stag;
	steering->mbva = mbva >> 8;
	steering->length = length;
	dma_wmb();
	steering->enable = 1;
	return 0;
}

int tof_core_cq_cacheflush(int tni, int cqid);

#define TOF_IB_TNI_OFFSET 3
#define TOF_IB_KCQ (TOF_ICC_NCQS - 1)
#define TOF_IB_ROUTE_CHECK_STAG 0
#define TOF_IB_ROUTE_CHECK_DMAADDR 0

#define TOF_IB_SEND_MTU (7 * 256 - sizeof(struct tof_ib_send_header))
#define TOF_IB_SEND_MAXLEN (32 * TOF_IB_SEND_MTU)

#define TOF_IB_TIMER_DELAY 1

#define TOF_IB_MAX_STAG 0x4000

#define TOF_IB_MAX_QPNO 800000
#define TOF_IB_MAX_QPID 4

static void tof_ib_steering_disable(int stag){
	struct tof_icc_steering_entry *steering = &tof_ib_steering[stag];
	struct tof_icc_mb_entry *mb = &tof_ib_mb[stag];
	steering->enable = 0;
	dma_wmb();
	mb->enable = 0;
	dma_wmb();
	tof_core_cq_cacheflush(TOF_IB_TNI_OFFSET, TOF_IB_KCQ);
	tof_core_cq_cacheflush(TOF_IB_TNI_OFFSET + 1, TOF_IB_KCQ);
}

static inline uint64_t tof_ib_dmaaddr_pack(uint32_t stag, uint32_t offset){
	return (uint64_t)stag << 32 | offset;
}

static inline uint32_t tof_ib_dmaaddr_stag(uint64_t dmaaddr){
	return dmaaddr >> 32;
}

/*
 * McKernel scatterlist is simply a contiguous buffer
 * This greatly simplifes dealing with it.
 */
struct scatterlist {
	void *pages;
	unsigned int	offset;
	unsigned int	length;
	unsigned long	dma_address;
	unsigned int	dma_length;
};

#if 0
static int tof_ib_map_sg(struct scatterlist *sg, int nents){
	struct tof_icc_mbpt_entry *mbpt;
	int stag;
	int ret;
	int i;
	int nr_pages;

	//if(!tof_ib_sg_is_contiguous(sg, nents)){
	//	tof_info(7002, "SG is not contiguous\n");
	//	return 0;
	//}

	for(i = 0; ; i++){
		stag = tof_ib_stag_alloc();
		if(stag >= 0){
			break;
		}
		if(i % 10000 == 0){
			//tof_warn(6013, "Cannot allocate STag\n");
			kprintf("%s: WARNING: cannot allocate STag\n", __func__);
		}
		//schedule();
	}

	//ret = tof_util_aligned_alloc(&tof_ib_mbpt_mem[stag], nents * TOF_ICC_MBPT_SIZE, TOF_ICC_MBPT_ALIGN);
	//if(ret < 0){
	//	tof_ib_stag_free(stag);
	//	return 0;
	//}

	nr_pages = (nents * TOF_ICC_MBPT_SIZE + (PAGE_SIZE - 1)) / PAGE_SIZE;
	tof_ib_mbpt_mem[stag].mem = ihk_mc_alloc_pages(nr_pages, IHK_MC_AP_NOWAIT);
	if (!tof_ib_mbpt_mem[stag].mem) {
		tof_ib_stag_free(stag);
		return 0;
	}
	tof_ib_mbpt_mem[stag].nr_pages = nr_pages;
	tof_ib_mbpt_mem[stag].offset = 0;

	mbpt = tof_ib_mbpt_mem[stag].mem;
	for(i = 0; i < nents; i++){
		//uint64_t paddr = sg_phys(&sg[i]) - sg[i].offset;
		uint64_t paddr = virt_to_phys(sg->pages) + i * PAGE_SIZE;
		mbpt[i].ipa = paddr >> 12;
		mbpt[i].enable = 1;
		//sg[i].dma_address = tof_ib_dmaaddr_pack(stag, i * PAGE_SIZE + sg[i].offset);
		//sg[i].dma_length = sg[i].length;
	}
	sg->dma_address = tof_ib_dmaaddr_pack(stag, 0);
	sg->dma_length = sg->length;

	//ret = tof_ib_steering_enable(stag, tof_util_get_pa(mbpt), nents, (size_t)nents << PAGE_SHIFT, 0);
	ret = tof_ib_steering_enable(stag, virt_to_phys(mbpt), nents, (size_t)nents << PAGE_SHIFT, 0);
	if(ret < 0){
		/* something going wrong */
		tof_ib_stag_free(stag);
		//tof_util_aligned_free(&tof_ib_mbpt_mem[stag]);
		ihk_mc_free_pages(tof_ib_mbpt_mem[stag].mem, nr_pages);
		return 0;
	}
	return nents;
}
#endif

static void tof_ib_unmap_sg(struct scatterlist *sg, int nents){
	int stag;
	//if(!tof_ib_sg_is_contiguous(sg, nents)){
	//	tof_info(7002, "SG is not contiguous\n");
	//	return;
	//}
	stag = tof_ib_dmaaddr_stag(sg->dma_address);
	tof_ib_steering_disable(stag);
	tof_ib_stag_free(stag);
	//tof_util_aligned_free(&tof_ib_mbpt_mem[stag]);
	ihk_mc_free_pages(tof_ib_mbpt_mem[stag].mem,
		tof_ib_mbpt_mem[stag].nr_pages);
	tof_ib_mbpt_mem[stag].mem = NULL;
	tof_ib_mbpt_mem[stag].nr_pages = 0;
}


static int tof_utofu_alloc_mbpt(struct tof_utofu_cq *ucq, uint32_t npages, struct tof_utofu_mbpt **pmbpt, int stag){
	size_t nsgents = npages / (PAGE_SIZE >> TOF_ICC_MBPT_SIZE_BITS);
	//int i;
	int ret;
	struct scatterlist *sg;
	struct tof_utofu_mbpt *mbpt;

	//sg = tof_util_alloc(nsgents * sizeof(*sg), GFP_ATOMIC);
	sg = kmalloc(sizeof(*sg), IHK_MC_AP_NOWAIT);
	if(sg == NULL){
		raw_rc_output(-ENOMEM);
		return -ENOMEM;
	}
	memset(sg, 0, sizeof(*sg));

	//sg_init_table(sg, nsgents);
	//for(i = 0; i < nsgents; i++){
	//	void *buf;
	//	buf = (void *)tof_util_get_free_pages(GFP_ATOMIC, 0);
	//	if(buf == NULL){
	//		ret = -ENOMEM;
	//		raw_rc_output(ret);
	//		goto free_ent;
	//	}
	//	memset(buf, 0, PAGE_SIZE);
	//	sg_set_buf(&sg[i], buf, PAGE_SIZE);
	//}
	sg->pages = ihk_mc_alloc_pages(nsgents, IHK_MC_AP_NOWAIT);
	if (!sg->pages) {
		raw_rc_output(-ENOMEM);
		ret = -ENOMEM;
		goto free_sg;
	}
	memset(sg->pages, 0, PAGE_SIZE * nsgents);

	//mbpt = tof_util_alloc(sizeof(*mbpt), GFP_ATOMIC);
	mbpt = kmalloc(sizeof(*mbpt), IHK_MC_AP_NOWAIT);
	if(mbpt == NULL){
		raw_rc_output(-ENOMEM);
		ret = -ENOMEM;
		goto free_sg_pages;
	}

	//ret = tof_smmu_iova_map_sg(ucq->tni, ucq->cqid, sg, nsgents);
	//if(ret == 0){
	//	ret = -EINVAL;
	//	goto free_ent;
	//}

	sg->dma_address = -1;
	{
		unsigned long phys = virt_to_phys(sg->pages);
		int i;

		for (i = 0; i < ihk_mc_get_nr_memory_chunks(); ++i) {
			unsigned long start, end;

			ihk_mc_get_memory_chunk(i, &start, &end, NULL);

			// Since chunks are contiguous, if end falls in,
			// the whole region is covered..
			if (phys < start || phys > end) {
				continue;
			}

			ihk_mc_get_memory_chunk_dma_addr(i, ucq->tni, ucq->cqid,
					(uintptr_t *)&sg->dma_address);
			sg->dma_address += (phys - start);
			break;
		}
	}

	if (sg->dma_address == -1) {
		kprintf("%s: error: obtaining sg DMA address\n", __func__);
		ret = -EINVAL;
		goto free_ent;
	}

	//atomic64_inc((atomic64_t *)&kref_init_count);
	kref_init(&mbpt->kref);
	mbpt->ucq = ucq;
	//mbpt->iova = sg_dma_address(sg);
	mbpt->iova = sg->dma_address;
	mbpt->sg = sg;
	mbpt->nsgents = nsgents;
	*pmbpt = mbpt;
	dkprintf("%s: mbpt iova: 0x%lx\n", __func__, mbpt->iova);

	return 0;
free_ent:
	//for(i = i - 1; i >= 0; i--){
	//	tof_util_free_pages((unsigned long)sg_virt(&sg[i]), 0);
	//}
	kfree(mbpt);
free_sg_pages:
	ihk_mc_free_pages(sg->pages, nsgents);
free_sg:
	kfree(sg);

	return ret;
}

static uintptr_t tof_utofu_disable_mbpt(struct tof_utofu_mbpt *mbpt, int idx){
	int i0, i1;
	struct tof_icc_mbpt_entry *ent;
	uintptr_t ipa;
	i0 = idx / (PAGE_SIZE / TOF_ICC_MBPT_SIZE);
	i1 = idx - i0 * (PAGE_SIZE / TOF_ICC_MBPT_SIZE);
	//ent = sg_virt(&mbpt->sg[i0]);
	ent = mbpt->sg->pages + (i0 * PAGE_SIZE);
	if(!ent[i1].enable){
		return 0;
	}
	ent[i1].enable = 0;
	ipa = (uint64_t)ent[i1].ipa << 12;
	ent[i1].ipa = 0;
	return ipa;
}

static void tof_utofu_enable_mbpt(struct tof_utofu_mbpt *mbpt, int idx, uintptr_t iova){
	int i0, i1;
	struct tof_icc_mbpt_entry *ent;
	i0 = idx / (PAGE_SIZE / TOF_ICC_MBPT_SIZE);
	i1 = idx - i0 * (PAGE_SIZE / TOF_ICC_MBPT_SIZE);
	//ent = sg_virt(&mbpt->sg[i0]);
	ent = mbpt->sg->pages + (i0 * PAGE_SIZE);
	ent[i1].ipa = iova>>12;
	dma_wmb();
	ent[i1].enable = 1;
}

static struct tof_icc_mbpt_entry *tof_utofu_get_mbpt_entry(struct tof_utofu_mbpt *mbpt, int idx){
	int i0, i1;
	struct tof_icc_mbpt_entry *ent;
	i0 = idx / (PAGE_SIZE / TOF_ICC_MBPT_SIZE);
	i1 = idx - i0 * (PAGE_SIZE / TOF_ICC_MBPT_SIZE);
	//ent = sg_virt(&mbpt->sg[i0]);
	ent = mbpt->sg->pages + (i0 * PAGE_SIZE);
	return &(ent[i1]);
}

static bool tof_utofu_mbpt_is_enabled(struct tof_utofu_mbpt *mbpt, int idx) {
	struct tof_icc_mbpt_entry *ent = tof_utofu_get_mbpt_entry(mbpt, idx);
	return (ent->enable == 1);
}

static int tof_utofu_update_mbpt_entries(struct tof_utofu_cq *ucq,
		struct tof_utofu_mbpt *mbpt,
		uintptr_t start,
		uintptr_t end,
		uint32_t ix,
		size_t pgsz,
		bool readonly)
{
	//struct page *page;
	struct process *proc = cpu_local_var(current)->proc;
	uintptr_t iova = 0, va;
	int ret;
	unsigned long phys = 0;

	/* Special case for straight mapping */
	if (proc->straight_va && (void *)start >= proc->straight_va &&
			(void *)end < proc->straight_va + proc->straight_len) {

		for (va = start; va < end; va += pgsz, ix++) {
			if (tof_utofu_mbpt_is_enabled(mbpt, ix)) {
				/* this page is already mapped to mbpt */
				kprintf("%s: 0x%lx already mapped...\n", __func__, va);
				continue;
			}

			/* Not yet resolved? */
			if (!iova) {
				int i;

				phys = proc->straight_pa +
					((void *)va - proc->straight_va);

				iova = -1;
				for (i = 0; i < ihk_mc_get_nr_memory_chunks(); ++i) {
					unsigned long start, end;

					ihk_mc_get_memory_chunk(i, &start, &end, NULL);

					if (phys < start || phys > end) {
						continue;
					}

					ihk_mc_get_memory_chunk_dma_addr(i, ucq->tni, ucq->cqid,
							(uintptr_t *)&iova);
					iova += (phys - start);
					break;
				}

				if (iova == -1) {
					return -EINVAL;
				}
			}

			tof_utofu_enable_mbpt(mbpt, ix, iova);
			iova += pgsz;
		}

		return 0;
	}

	for(va = start; va < end; va += pgsz, ix++){

		if (tof_utofu_mbpt_is_enabled(mbpt, ix)) {
			/* this page is already mapped to mbpt */
			continue;
		}

		//ret = get_user_pages(va, 1, readonly ? 0 : FOLL_WRITE, &page, NULL);
		//if(ret < 1){
		//	raw_rc_output(ret);
		//	if(tof_utofu_stag_debug & 0x4){
		//		tof_info(9999, "[%s] get_user_pages: ret=%d va=0x%lx readonly=%d\n", current->comm, ret, va, readonly);
		//	}
		//	if(ret == -EFAULT && !readonly){
		//		return -EPERM;
		//	}
		//	return -ENOMEM;
		//}

		ret = ihk_mc_pt_virt_to_phys(
				cpu_local_var(current)->vm->address_space->page_table,
				(void *)va, &phys);

		if (ret) {
			raw_rc_output(ret);
			return -ENOMEM;
		}

		//iova = tof_smmu_get_ipa_cq(ucq->tni, ucq->cqid,
		//			   pfn_to_kaddr(page_to_pfn(page)), pgsz);
		//if (iova == 0) {
		//	put_page(page);
		//	raw_rc_output(ret);
		//	return -ENOMEM;
		//}

		iova = -1;
		{
			int i;
			for (i = 0; i < ihk_mc_get_nr_memory_chunks(); ++i) {
				unsigned long start, end;

				ihk_mc_get_memory_chunk(i, &start, &end, NULL);

				if (phys < start || phys > end) {
					continue;
				}

				ihk_mc_get_memory_chunk_dma_addr(i, ucq->tni, ucq->cqid,
						(uintptr_t *)&iova);
				iova += (phys - start);
				break;
			}
		}

		if (iova == -1) {
			return -EINVAL;
		}

		dkprintf("%s: VA: 0x%lx -> iova (phys): 0x%lx\n",
				__func__, va, phys);

		/* Check ovalap MBPT IOVA */
		//ret = tof_utofu_check_overlap_mbpt_iova(iova, ucq, mbpt, ix);
		//if(unlikely(ret)){
		//	put_page(page);
		//	return ret;
		//}

		tof_utofu_enable_mbpt(mbpt, ix, iova);
		//put_page(page);
	}
	return 0;
}

static void tof_utofu_free_mbpt(struct tof_utofu_cq *ucq, struct tof_utofu_mbpt *mbpt){
	int i;

	for(i = 0; i < mbpt->nsgents * PAGE_SIZE / sizeof(struct tof_icc_mbpt_entry); i++){
		tof_utofu_disable_mbpt(mbpt, i);
		//uintptr_t iova;
		//iova = tof_utofu_disable_mbpt(mbpt, i);
		//if(iova){
			/* This appears to be doing nothing, see tof_ib_dma_ops->unmap_page */
			//tof_smmu_release_ipa_cq(ucq->tni, ucq->cqid, iova, mbpt->pgsz);
		//}
	}

	//tof_smmu_iova_unmap_sg(ucq->tni, ucq->cqid, mbpt->sg, mbpt->nsgents);
	// Do nothing in McKernel..

	//for(i = 0; i < mbpt->nsgents; i++){
	//	tof_util_free_pages((unsigned long)sg_virt(&mbpt->sg[i]), 0);
	//}
	ihk_mc_free_pages(mbpt->sg->pages, mbpt->nsgents);

	//tof_util_free(mbpt->sg);
	kfree(mbpt->sg);

	//tof_util_free(mbpt);
	kfree(mbpt);
	dkprintf("%s: mbpt %p freed\n", __func__, mbpt);
}

static void tof_utofu_enable_steering(struct tof_utofu_cq *ucq, int stag, uintptr_t mbva, size_t length, bool readonly){
	struct tof_icc_steering_entry *steering = &ucq->steering[stag];

	steering->length = length;
	steering->readonly = readonly;
	steering->mbva = mbva>>8;
	steering->mbid = stag;
	dma_wmb();
	steering->enable = 1;
}

static void tof_utofu_enable_mb(struct tof_utofu_cq *ucq, int stag, uintptr_t iova, uint8_t pgszbits, size_t npages){
	struct tof_icc_mb_entry *mb = &ucq->mb[stag];

	mb->npage = npages;
	mb->ps = TOF_ICC_MB_PS_ENCODE(pgszbits);
	mb->ipa = iova>>8;
	dma_wmb();
	mb->enable = 1;
}

static void tof_utofu_trans_mru_delete(struct tof_utofu_cq *ucq, int stag){
	struct tof_utofu_trans_list *mru = ucq->trans.mru;
	int prev = mru[stag].prev;
	int next = mru[stag].next;
	if(prev == TOF_UTOFU_MRU_EMPTY || next == TOF_UTOFU_MRU_EMPTY){ /* already deleted */
		return;
	}
	if(prev == stag){  /* a single entry */
		ucq->trans.mruhead = TOF_UTOFU_MRU_EMPTY;
	}else{
		if(ucq->trans.mruhead == stag){
			ucq->trans.mruhead = next;
		}
		mru[prev].next = next;
		mru[next].prev = prev;
	}
	mru[stag].prev = TOF_UTOFU_MRU_EMPTY;
	mru[stag].next = TOF_UTOFU_MRU_EMPTY;
}

static void tof_utofu_trans_mru_insert(struct tof_utofu_cq *ucq, int stag, uint8_t pgszbits, struct tof_utofu_mbpt *mbpt){
	struct tof_utofu_trans_list *mru = ucq->trans.mru;
	mru[stag].pgszbits = pgszbits;
	mru[stag].mbpt = mbpt;
	if(ucq->trans.mruhead == TOF_UTOFU_MRU_EMPTY){
		mru[stag].prev = stag;
		mru[stag].next = stag;
	}else{
		int next = ucq->trans.mruhead;
		int prev = mru[next].prev;
		mru[stag].prev = prev;
		mru[stag].next = next;
		mru[prev].next = stag;
		mru[next].prev = stag;
	}
	ucq->trans.mruhead = stag;
}

static void tof_utofu_trans_update(struct tof_utofu_cq *ucq, int stag, uintptr_t start, size_t len, uint8_t pgszbits, struct tof_utofu_mbpt *mbpt){
	struct tof_trans_table *table = ucq->trans.table;
	union {
		struct tof_trans_table ent;
		uint64_t atomic;
	} tmp;
	tmp.ent.steering.bits.start = start >> PAGE_SHIFT;
	tmp.ent.steering.bits.len = len >> PAGE_SHIFT;
	tmp.ent.steering.bits.ps_code = (pgszbits == PAGE_SHIFT)? TOF_STAG_TRANS_PS_CODE_64KB:TOF_STAG_TRANS_PS_CODE_2MB;
	//atomic64_set((atomic64_t *)&table[stag], tmp.atomic);
	ihk_atomic64_set((ihk_atomic64_t *)&table[stag], tmp.atomic);

	linux_spin_lock(&ucq->trans.mru_lock);
	tof_utofu_trans_mru_delete(ucq, stag);
	tof_utofu_trans_mru_insert(ucq, stag, pgszbits, mbpt);
	linux_spin_unlock(&ucq->trans.mru_lock);
}



static void tof_utofu_trans_disable(struct tof_utofu_cq *ucq, int stag){
	struct tof_trans_table *table = ucq->trans.table;
	//atomic64_set((atomic64_t *)&table[stag], 0);
	ihk_atomic64_set((ihk_atomic64_t *)&table[stag], 0);
	tof_utofu_trans_mru_delete(ucq, stag);
}

static void tof_utofu_trans_enable(struct tof_utofu_cq *ucq, int stag, uintptr_t start, size_t len, uintptr_t mbptstart, size_t mbptlen, uint8_t pgszbits, struct tof_utofu_mbpt *mbpt){
	struct tof_trans_table *table = ucq->trans.table;
	table[stag].mbpt.bits.start = mbptstart >> PAGE_SHIFT;
	table[stag].mbpt.bits.len = mbptlen >> PAGE_SHIFT;
	table[stag].mbpt.bits.ps_code = (pgszbits == PAGE_SHIFT)? TOF_STAG_TRANS_PS_CODE_64KB:TOF_STAG_TRANS_PS_CODE_2MB;
	wmb();
	tof_utofu_trans_update(ucq, stag, start, len, pgszbits, mbpt);
}

static int tof_utofu_alloc_new_steering(struct tof_utofu_cq *ucq, int stag, uintptr_t start, uintptr_t end, uint8_t pgszbits, uintptr_t plus_mbva, bool readonly){
	uintptr_t mbptstart;
	size_t pgsz = (size_t)1 << pgszbits;
	size_t npages, mbpt_npages;
	uint32_t ix;
	int ret;
	struct tof_utofu_mbpt *mbpt;
	uintptr_t mbva;

	npages = (end - start) >> pgszbits;
	mbpt_npages = roundup(npages, PAGE_SIZE / TOF_ICC_MBPT_SIZE);
	ret = tof_utofu_calc_mbptstart((int64_t)start, (int64_t)end, mbpt_npages, pgszbits, &mbptstart);
	if (ret < 0) {
		raw_rc_output(ret);
		return ret;
	}

	ret = tof_utofu_alloc_mbpt(ucq, mbpt_npages, &mbpt, stag);
	if(ret < 0){
		raw_rc_output(ret);
		return ret;
	}
	mbpt->mbptstart = mbptstart;
	mbpt->pgsz = pgsz;

	ix = (start - mbptstart) >> pgszbits;
	ret = tof_utofu_update_mbpt_entries(ucq, mbpt, start, end, ix, pgsz, readonly);
	if (ret < 0) {
		raw_rc_output(ret);
		//if(ret == -EFAULT){
		//	tof_warn(9999, "Founds the overlap MBPT iova. abnormal end. Target TNI=%d CQ=%d Stag[%d] comm=%s pid=%d\n"
		//		,ucq->tni, ucq->cqid, stag, current->comm, current->pid);
		//}
		tof_utofu_free_mbpt(ucq, mbpt);
		return ret;
	}

	if(plus_mbva == TOF_UTOFU_BLANK_MBVA) {
		mbva = 0;
	} else {
		mbva = start - mbptstart + plus_mbva;
	}
	//if(tof_utofu_stag_debug & 0x1){
	//	tof_info(9999, "[%s] tni=%d cq=%d stag=%d mbva=%ld start=0x%lx end=0x%lx mbptstart=0x%lx npages=%ld mbpt_npages=%ld plus_mbva=%ld pgszbits=%d\n", current->comm, ucq->tni, ucq->cqid, stag, mbva, start, end, mbptstart, npages, mbpt_npages, plus_mbva, pgszbits);
	//}
	tof_utofu_enable_mb(ucq, stag, mbpt->iova, pgszbits, mbpt_npages);
	tof_utofu_enable_steering(ucq, stag, mbva, end - mbptstart - mbva, readonly);
	tof_utofu_trans_enable(ucq, stag, start, end - start, mbptstart, mbpt_npages * TOF_ICC_MBPT_SIZE, pgszbits, mbpt);

	return 0;
}

static void tof_utofu_release_stag(struct tof_utofu_cq *ucq, int stag){
	/* nothing to do */
	/* tof_utofu_reserve_stag() and tof_utofu_release_stag() are in a same ucq_lock region */
	return;
}

static int tof_utofu_ioctl_alloc_stag(struct tof_utofu_device *dev, unsigned long arg) {
	struct tof_utofu_cq *ucq;
	struct tof_alloc_stag req;
	struct process_vm *vm = cpu_local_var(current)->vm;
	bool readonly;
	uintptr_t start;
	uintptr_t end;
	uint8_t pgszbits;
	size_t pgsz;
	int ret = -ENOTSUPP;

	ucq = container_of(dev, struct tof_utofu_cq, common);
	if(!ucq->common.enabled){
		return -EPERM;
	}
	if(copy_from_user(&req, (void *)arg, sizeof(req)) != 0){
		return -EFAULT;
	}
	dkprintf("%s: [IN] tni=%d cqid=%d flags=%u stag=%d va=%p len=%llx\n",
		__func__, ucq->tni, ucq->cqid, req.flags, req.stag, req.va, req.len);

	if(req.stag < -1 || req.stag >= TOF_UTOFU_SPECIAL_STAG ||
	   req.va == NULL || req.len == 0){
		return -EINVAL;
	}
	dkprintf("%s: ucq->steering: 0x%lx\n", __func__, ucq->steering);
	if(req.stag >= 0 && ucq->steering[req.stag].enable){
		return -EBUSY;
	}

	readonly = (req.flags & 1) != 0;
	ihk_rwspinlock_read_lock_noirq(&vm->memory_range_lock);
	pgszbits = PAGE_SHIFT;
	if (req.flags & TOF_UTOFU_ALLOC_STAG_LPG) {
		ret = tof_utofu_get_pagesize_locked((uintptr_t)req.va,
				req.len, &pgszbits, readonly);
		if(ret < 0){
			kprintf("%s: ret: %d\n", __func__, ret);
			ihk_rwspinlock_read_unlock_noirq(&vm->memory_range_lock);
			return ret;
		}
	}
	pgsz = (size_t)1 << pgszbits;
	start = round_down((uintptr_t)req.va, pgsz);
	end = round_up((uintptr_t)req.va + req.len, pgsz);
	dkprintf("%s: 0x%lx:%llu, start: 0x%lx, end: 0x%lx, pgsz: %d\n",
		__func__, req.va, req.len, start, end, pgsz);

	//down(&ucq->ucq_sem);
	if(req.stag < 0){
#if 1
		/* normal stag */
		int stag;
		linux_spin_lock(&ucq->trans.mru_lock);
		stag = tof_utofu_trans_search(ucq, start, end, pgszbits, readonly);
		linux_spin_unlock(&ucq->trans.mru_lock);
		if(stag < 0){
			struct tof_utofu_mbpt *mbpt = NULL;
			stag = tof_utofu_reserve_stag(ucq, readonly);
			if(stag < 0){
				//up(&ucq->ucq_sem);
				ihk_rwspinlock_read_unlock_noirq(&vm->memory_range_lock);
				return -ENOSPC;
			}

			/* With tof_utofu_disable_extend, this call does nothing */
			//spin_lock(&ucq->trans.mru_lock);
			//mbpt = tof_utofu_mbpt_search(ucq, start, end, readonly, pgszbits);
			//spin_unlock(&ucq->trans.mru_lock);
			if (mbpt == NULL) {
				ret = tof_utofu_alloc_new_steering(ucq, stag, start, end, pgszbits,
					TOF_UTOFU_BLANK_MBVA, readonly);
			}
			//else {
			//	ret = tof_utofu_extend_steering(ucq, stag, mbpt, start, end, pgszbits, readonly);
			//}
			if(ret < 0){
				tof_utofu_release_stag(ucq, stag);
			}
		}
		else{
			ret = 0;
		}
		req.stag = stag;
		req.offset = (uintptr_t)req.va - tof_utofu_get_mbpt_start(ucq, stag);
#endif
	}
	else{
		/* special stag */
		uintptr_t plus_mbva;
		if(ucq->steering[req.stag].enable){
			kprintf("%s: ret: %d\n", __func__, -EBUSY);
			//up(&ucq->ucq_sem);
			ihk_rwspinlock_read_unlock_noirq(&vm->memory_range_lock);
			return -EBUSY;
		}
		plus_mbva = round_down((uintptr_t)req.va, 256) - start;
		ret = tof_utofu_alloc_new_steering(ucq, req.stag, start, end, pgszbits, plus_mbva, readonly);
		req.offset = (uintptr_t)req.va & 0xff;
	}

	//up(&ucq->ucq_sem);
	ihk_rwspinlock_read_unlock_noirq(&vm->memory_range_lock);

	if(ret == 0){
		if(copy_to_user((void *)arg, &req, sizeof(req)) != 0){
			kprintf("%s: ret: %d\n", __func__, -EFAULT);
			ret = -EFAULT;
		}
	}

	//if(unlikely(tof_utofu_stag_debug & 0x100)){
	//	tof_info(9999, "[%s] ucq=%d:%d stag=%d offset=%llu va=%p len=%llu flags=%d\n",
	//		current->comm, ucq->tni, ucq->cqid, req.stag, req.offset, req.va, req.len, req.flags);
	//}

	dkprintf("%s: [OUT] tni=%d cqid=%d stag=%d offset=0x%llx ret=%d\n",
		__func__, ucq->tni, ucq->cqid, req.stag, req.offset, ret);
	return ret;
}

static void tof_utofu_mbpt_release(struct kref *kref)
{
	struct tof_utofu_mbpt *mbpt = container_of(kref, struct tof_utofu_mbpt, kref);
	//atomic64_inc((atomic64_t *)&kref_free_count);
	tof_utofu_free_mbpt(mbpt->ucq, mbpt);
}

//static struct tof_core_cq tof_core_cq[TOF_ICC_NTNIS][TOF_ICC_NCQS];
static struct tof_core_cq *tof_core_cq;

struct tof_core_cq *tof_core_cq_get(int tni, int cqid){
	if((unsigned int)tni >= TOF_ICC_NTNIS ||
	   (unsigned int)cqid >= TOF_ICC_NCQS){
		return NULL;
	}
	//return tof_core_cq[tni][cqid];

	// Convert [][] notion into pointer aritmethic
	return tof_core_cq + (tni * TOF_ICC_NCQS) + cqid;
}

static inline void tof_writeq_relaxed(uint64_t val, void *reg, off_t offset){
	writeq_relaxed(val, (char *)reg + offset);
}

static inline uint64_t tof_readq(void *reg, off_t offset){
	return readq((char *)reg + offset);
}

static inline bool tof_core_readq_spin(void *reg, off_t offset, uint64_t mask,
				uint64_t expect, unsigned long timeout){
	uint64_t val;
	unsigned long cyc;
	cyc = rdtsc();
	do{
		val = tof_readq(reg, offset);
		if(rdtsc() - cyc > timeout){
			return false;
		}
	}while((val & mask) != expect);
	return true;
}

static int tof_core_cq_cache_flush_timeout_panic_disabled = 1;
static int tof_core_cq_cacheflush_is_cqs_steering_table_bit_disabled = 1;

#define TOF_CORE_KCQID (TOF_ICC_NCQS - 1)

static int tof_core_cacheflush_timeout(struct tof_core_cq *timeout_cq){
	int tni, cqid;

	for(tni = 0; tni < TOF_ICC_NTNIS; tni++){
		for(cqid = 0; cqid < TOF_ICC_NCQS; cqid++){
			struct tof_core_cq *cq  = tof_core_cq_get(tni, cqid);

			if(cqid == TOF_CORE_KCQID){
				continue;
			}
			/* write 0 to steering table enable bit of CQS reg -> MRQ RCODE 10h issued */
			if(tof_core_cq_cacheflush_is_cqs_steering_table_bit_disabled){
				tof_writeq_relaxed(0, cq->reg.cqs, TOF_ICC_REG_CQS_STEERING_TABLE_ENABLE);
				wmb();
			}
			/* send signal */
			//if(tof_core_cq_cacheflush_is_send_signal_enabled){
			//	tof_core_irq_handler_cq_user(&cq->irq, TOF_ICC_DUMMY_IRQ_CQS_CACHEFLUSH_TIMEOUT, timeout_cq);
			//}
			kprintf("%s WARNING: no signal sent.. \n", __func__);
		}
	}
	return 0;
}

static int   tof_core_cq_cache_flush_timeout_sec = 3;
static int   tof_core_cq_cache_flush_2nd_timeout_sec = 3600;
int tof_core_cq_cacheflush_timeout_dbg_msg_disabled = 1;

// Assuming 2 GHz..
#define TOF_CORE_TIMEOUT_SEC(n) ((n) * 2 * 1000000000)

int tof_core_cq_cacheflush(int tni, int cqid){
	struct tof_core_cq *cq;
	cq = tof_core_cq_get(tni, cqid);
	tof_writeq_relaxed(1, cq->reg.cqs, TOF_ICC_REG_CQS_CACHE_FLUSH);
	if(!tof_core_readq_spin(cq->reg.cqs, TOF_ICC_REG_CQS_STATUS,
				TOF_ICC_REG_CQS_STATUS_CACHE_FLUSH_BUSY,
				0, TOF_CORE_TIMEOUT_SEC(tof_core_cq_cache_flush_timeout_sec))){
 		if(likely(tof_core_cq_cache_flush_timeout_panic_disabled)){

			//tof_warn(2018, "cache flush timeout: tni=%d cqid=%d", tni, cqid);
			kprintf("%s: cache flush timeout: tni=%d cqid=%d", __func__, tni, cqid);

			/* cacheflush timeout processing for user CQ in TNI */
			tof_core_cacheflush_timeout(cq);

			/* Check cacheflush status change */
			if(!tof_core_readq_spin(cq->reg.cqs, TOF_ICC_REG_CQS_STATUS,
					TOF_ICC_REG_CQS_STATUS_CACHE_FLUSH_BUSY,
					0, TOF_CORE_TIMEOUT_SEC(tof_core_cq_cache_flush_2nd_timeout_sec))){
				//tof_info(9999, "been exceeded cacheflush timeout status check time=%d : tni=%d cqid=%d",tof_core_cq_cache_flush_2nd_timeout_sec,tni,cqid);
				//tof_panic(8, "cache flush timeout: tni=%d cqid=%d", tni, cqid);
				kprintf("%s: cache flush timeout: tni=%d cqid=%d", __func__, tni, cqid);
				panic("cache flush timeout");
			}
			else{
				//if(!tof_core_cq_cacheflush_timeout_dbg_msg_disabled){
				//	tof_info(9999, "been changed within cacheflush timeout status check time=%d : tni=%d cqid=%d",tof_core_cq_cache_flush_2nd_timeout_sec,tni,cqid);
				//}
			}
		}else{
			//tof_panic(8, "cache flush timeout: tni=%d cqid=%d", tni, cqid);
			kprintf("%s: cache flush timeout: tni=%d cqid=%d", __func__, tni, cqid);
			panic("cache flush timeout");
		}
	}
	return 0;
}

static int tof_utofu_cq_cacheflush(struct tof_utofu_cq *ucq){
	return tof_core_cq_cacheflush(ucq->tni, ucq->cqid);
}


static int tof_utofu_free_stag(struct tof_utofu_cq *ucq, int stag){
	if(stag < 0 || stag >= TOF_UTOFU_NUM_STAG(ucq->num_stag) ||
	   ucq->steering == NULL){
		return -EINVAL;
	}
	if(!(ucq->steering[stag].enable)){
		return -ENOENT;
	}
	if (!kref_is_mckernel(&ucq->trans.mru[stag].mbpt->kref)) {
		kprintf("%s: stag: %d is not an McKernel kref\n", __func__, stag);
		return -EINVAL;
	}
	//if(unlikely(tof_utofu_stag_debug & 0x20)){
	//	tof_info(9999, "[%s] ucq=%d:%d stag=%d\n", current->comm, ucq->tni, ucq->cqid, stag);
	//}
	ucq->steering[stag].enable = 0;
	ucq->mb[stag].enable = 0;
	tof_utofu_trans_disable(ucq, stag);
	dma_wmb();
	tof_utofu_cq_cacheflush(ucq);
	kref_put(&ucq->trans.mru[stag].mbpt->kref, tof_utofu_mbpt_release);
	ucq->trans.mru[stag].mbpt = NULL;
	dkprintf("%s: stag: %d deallocated\n", __func__, stag);
	return 0;
}


static int tof_utofu_ioctl_free_stags(struct tof_utofu_device *dev, unsigned long arg){
	struct tof_utofu_cq *ucq;
	struct tof_free_stags req;
	int i, no_free_cnt = 0, ret;

	ucq = container_of(dev, struct tof_utofu_cq, common);

	if(copy_from_user(&req, (void *)arg, sizeof(req)) != 0){
		raw_rc_output(-EFAULT);
		return -EFAULT;
	}
	//tof_log_if("[IN] tni=%d cqid=%d num=%u stags=%p\n", ucq->tni, ucq->cqid, req.num, req.stags);
	dkprintf("%: [IN] tni=%d cqid=%d num=%u stags=%p\n",
			__func__, ucq->tni, ucq->cqid, req.num, req.stags);

	if(req.num > 1024 || req.stags == NULL){
		return -EINVAL;
	}
	for(i = 0; i < req.num; i++){
		int stag;
		if(copy_from_user(&stag, &req.stags[i], sizeof(stag)) != 0){
			raw_rc_output(-EFAULT);
			return -EFAULT;
		}
		linux_spin_lock(&ucq->trans.mru_lock);
		ret = tof_utofu_free_stag(ucq, stag);
		linux_spin_unlock(&ucq->trans.mru_lock);
		if(ret == 0){
			int result = -1;
			if(copy_to_user(&req.stags[i], &result, sizeof(result)) != 0){
				raw_rc_output(-EFAULT);
				return -EFAULT;
			}
		}
		else if(ret == -ENOENT){
			no_free_cnt++;
			continue; /* continue free tag process */
		}
		else{
			req.num = i - no_free_cnt;
			if(copy_to_user((void *)arg, &req, sizeof(req)) != 0){
				return -EFAULT;
			}
			//tof_log_if("[OUT] tni=%d cqid=%d num=%u stags=%p ret=%d no_free_cnt=%d\n", ucq->tni, ucq->cqid, req.num, req.stags, ret, no_free_cnt);
			return ret;
		}
	}

	req.num = i - no_free_cnt;
	if(copy_to_user((void *)arg, &req, sizeof(req)) != 0){
		return -EFAULT;
	}
	//tof_log_if("[OUT] tni=%d cqid=%d num=%u stags=%p no_free_cnt=%d\n", ucq->tni, ucq->cqid, req.num, req.stags, no_free_cnt);
	dkprintf("%s: [OUT] tni=%d cqid=%d num=%u stags=%p no_free_cnt=%d\n",
		__func__, ucq->tni, ucq->cqid, req.num, req.stags, no_free_cnt);

	return no_free_cnt > 0 ? -ENOENT : 0;
}

void tof_utofu_release_cq(void *pde_data)
{
	struct tof_utofu_cq *ucq;
	int stag;
	struct tof_utofu_device *dev;

	dev = (struct tof_utofu_device *)pde_data;
	ucq = container_of(dev, struct tof_utofu_cq, common);

	if (!ucq->common.enabled) {
		kprintf("%s: UCQ TNI %d, CQ %d is disabled\n",
			__func__, ucq->tni, ucq->cqid);
		return;
	}

	for (stag = 0; stag < TOF_UTOFU_NUM_STAG(ucq->num_stag); stag++) {
		linux_spin_lock(&ucq->trans.mru_lock);
		tof_utofu_free_stag(ucq, stag);
		linux_spin_unlock(&ucq->trans.mru_lock);
	}

	kprintf("%s: UCQ (pde: %p) TNI %d, CQ %d\n",
		__func__, pde_data, ucq->tni, ucq->cqid);
}

long tof_utofu_unlocked_ioctl_cq(int fd,
		unsigned int cmd, unsigned long arg) {
	int ret = -ENOTSUPP;
	struct thread *thread = cpu_local_var(current);
	struct tof_utofu_device *dev;

	/* ENOTSUPP inidicates proceed with offload */
	if (fd >= MAX_FD_PDE || !thread->proc->fd_pde_data[fd]) {
		return -ENOTSUPP;
	}

	dev = (struct tof_utofu_device *)thread->proc->fd_pde_data[fd];

#if 0
	switch (cmd) {
		case TOF_IOCTL_INIT_CQ:
			kprintf("%s: TOF_IOCTL_INIT_CQ @ %d\n", __func__, fd);
			break;

		case TOF_IOCTL_ALLOC_STAG:
			kprintf("%s: TOF_IOCTL_ALLOC_STAG @ %d\n", __func__, fd);
			break;

		case TOF_IOCTL_FREE_STAGS:
			kprintf("%s: TOF_IOCTL_FREE_STAGS @ %d\n", __func__, fd);
			break;

		case TOF_IOCTL_SET_RT_SIGNAL:
			kprintf("%s: TOF_IOCTL_SET_RT_SIGNAL @ %d\n", __func__, fd);
			break;

		case TOF_IOCTL_GET_PORT_STAT:
			kprintf("%s: TOF_IOCTL_GET_PORT_STAT @ %d\n", __func__, fd);
			break;

		case TOF_IOCTL_GET_CQ_STAT:
			kprintf("%s: TOF_IOCTL_GET_CQ_STAT @ %d\n", __func__, fd);
			break;

		case TOF_IOCTL_ENABLE_BCH:
			kprintf("%s: TOF_IOCTL_ENABLE_BCH @ %d\n", __func__, fd);
			break;

		case TOF_IOCTL_DISABLE_BCH:
			kprintf("%s: TOF_IOCTL_DISABLE_BCH @ %d\n", __func__, fd);
			break;

		case TOF_IOCTL_SET_SUBNET:
			kprintf("%s: TOF_IOCTL_SET_SUBNET @ %d\n", __func__, fd);
			break;

		case TOF_IOCTL_REG_USER:
			kprintf("%s: TOF_IOCTL_REG_USER @ %d\n", __func__, fd);
			break;

		case TOF_IOCTL_NOTIFY_LINKDOWN:
			kprintf("%s: TOF_IOCTL_NOTIFY_LINKDOWN @ %d\n", __func__, fd);
			break;

		case TOF_IOCTL_LOAD_REGISTER:
			kprintf("%s: TOF_IOCTL_LOAD_REGISTER @ %d\n", __func__, fd);
			break;

		case TOF_IOCTL_LOAD_RESOURCE:
			kprintf("%s: TOF_IOCTL_LOAD_RESOURCE @ %d\n", __func__, fd);
			break;

		default:
			kprintf("%s: unknown @ %d\n", __func__, fd);
			break;
	}
#endif

	switch (cmd) {
		case TOF_IOCTL_ALLOC_STAG:
			ret = tof_utofu_ioctl_alloc_stag(dev, arg);
			break;
		case TOF_IOCTL_FREE_STAGS:
			ret = tof_utofu_ioctl_free_stags(dev, arg);
			break;
		default:
			ret = -ENOTSUPP;
	}

	return ret;
}

extern struct tofu_globals *ihk_mc_get_tofu_globals(void);
void tof_utofu_init_globals(void)
{
	struct tofu_globals *tg = ihk_mc_get_tofu_globals();

	tof_ib_stag_list = (int16_t *)tg->tof_ib_stag_list_addr;
	tof_ib_stag_lock = (ihk_spinlock_t *)tg->tof_ib_stag_lock_addr;
	tof_ib_stag_list_Rp_addr = (int *)tg->tof_ib_stag_list_Rp_addr;
	tof_ib_stag_list_Wp_addr = (int *)tg->tof_ib_stag_list_Wp_addr;
	tof_ib_mbpt_mem =
		(struct tof_util_aligned_mem *)tg->tof_ib_mbpt_mem_addr;
	tof_ib_steering =
		(struct tof_icc_steering_entry *)tg->tof_ib_steering_addr;
	tof_ib_mb =
		(struct tof_icc_mb_entry *)tg->tof_ib_mb_addr;
	tof_core_cq =
		(struct tof_core_cq *)tg->tof_core_cq_addr;

	dkprintf("%s: tof_ib_stag_lock: 0x%lx\n",
		__func__, tg->tof_ib_stag_lock_addr);
	dkprintf("%s: tof_ib_stag_list_Wp_addr: 0x%lx\n",
			__func__, tg->tof_ib_stag_list_Wp_addr);
	dkprintf("%s: tof_ib_stag_list_Wp: %d\n",
			__func__, *((int *)tg->tof_ib_stag_list_Wp_addr));
	kprintf("%s: linux_vmalloc_start: %p\n", __func__, tg->linux_vmalloc_start);
	kprintf("Tofu globals initialized.\n");
}


void tof_utofu_finalize(void)
{
	struct tofu_globals *tg = ihk_mc_get_tofu_globals();

	ihk_mc_clear_kernel_range((void *)tg->linux_vmalloc_start,
			(void *)tg->linux_vmalloc_end);
}