/*-
 * Copyright (c) 2008, David Xu <davidxu@freebsd.org>
 * 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 unmodified, 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 ``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 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/kernel.h>
#include <sys/lock.h>
#include <sys/mutex.h>
#include <sys/eventhandler.h>
#include <sys/proc.h>
#include <sys/sched.h>
#include <sys/schedctl.h>
#include <sys/syscallsubr.h>
#include <sys/sysent.h>
#include <sys/systm.h>
#include <sys/sysproto.h>
#include <sys/mman.h>
#include <vm/vm.h>
#include <vm/vm_extern.h>
#include <vm/vm_kern.h>
#include <vm/vm_page.h>
#include <vm/vm_map.h>
#include <vm/uma.h>

#define _NSC_PER_PAGE		(PAGE_SIZE/sizeof(sc_shared_t))
#define _NSC_BITS		(sizeof(long) * NBBY)
#define _NSC_WORDS		howmany(_NSC_PER_PAGE, _NSC_BITS)

#define PSCF_ALLOC		0x01
#define PSCF_WAIT		0x02

typedef struct sc_page {
	SLIST_ENTRY(sc_page)	scp_link;
	long			scp_bmp[_NSC_WORDS];
	int			scp_free;
	sc_shared_t		*scp_uaddr;
	sc_shared_t		*scp_kaddr;
	vm_page_t		scp_page;
} sc_page_t;

static uma_zone_t	sc_page_zone;

static void		schedctl_start(void);
static int		schedctl_shared_alloc(sc_shared_t **, caddr_t *);
static int		schedctl_alloc_page(sc_page_t **);
static void		schedctl_free_page(sc_page_t *);
static int		schedctl_alloc_bit(sc_page_t *);
static sc_page_t	*schedctl_find_page(sc_shared_t *, int *);
static void		schedctl_clear_bit(sc_page_t *, int);
static void		schedctl_proc_ctor(void *arg, struct proc *p);
static void		schedctl_proc_cleanup(void *);

SYSINIT(schedctl, SI_SUB_P1003_1B, SI_ORDER_ANY, schedctl_start, NULL);

static void
schedctl_start(void)
{
	sc_page_zone = uma_zcreate("sc_page", sizeof(sc_page_t),
		NULL, NULL, NULL, NULL, UMA_ALIGN_PTR, 0);
	EVENTHANDLER_REGISTER(process_ctor, schedctl_proc_ctor, NULL,
		EVENTHANDLER_PRI_ANY);
	EVENTHANDLER_REGISTER(process_exit, schedctl_proc_cleanup, NULL,
		EVENTHANDLER_PRI_ANY);
	EVENTHANDLER_REGISTER(process_exec, schedctl_proc_cleanup, NULL,
		EVENTHANDLER_PRI_ANY);
}

int
schedctl(struct thread *td, struct schedctl_args *uap)
{
	sc_shared_t	*ssp;
	caddr_t		uaddr;
	int		error;
	
	if (td->td_schedctl == NULL) {
		if ((error = schedctl_shared_alloc(&ssp, &uaddr)) != 0)
			return (error);
		bzero(ssp, sizeof(*ssp));

		thread_lock(td);
		td->td_schedctl = ssp;
		td->td_sc_uaddr = uaddr;
		td->td_schedctl->sc_state = SC_ONPROC;
		thread_unlock(td);
	}

	td->td_retval[0] = (register_t)(td->td_sc_uaddr);
	return (0);
}

static int
schedctl_shared_alloc(sc_shared_t **ssp, caddr_t *uaddr)
{
	struct proc	*p = curproc;
	sc_page_t	*scp;
	int		error, bit;
	int		flags;
	
	PROC_LOCK(p);
restart:
	SLIST_FOREACH(scp, &p->p_sc_list, scp_link) {
		if (scp->scp_free != 0)
			break;
	}

	if (scp == NULL) {
		if (p->p_sc_flags & PSCF_ALLOC) {
			while (p->p_sc_flags & PSCF_ALLOC) {
				p->p_sc_flags |= PSCF_WAIT;
				msleep(&p->p_sc_flags, &p->p_mtx, 0, "schedctl", 0);
			}
			goto restart;
		}

		p->p_sc_flags |= PSCF_ALLOC;
		PROC_UNLOCK(p);

		error = schedctl_alloc_page(&scp);

		PROC_LOCK(p);
		flags = p->p_sc_flags;
		p->p_sc_flags &= ~(PSCF_ALLOC|PSCF_WAIT);
		if (flags & PSCF_WAIT)
			wakeup(&p->p_sc_flags);
		if (error) {
			PROC_UNLOCK(p);
			return (error);
		}
			
		SLIST_INSERT_HEAD(&p->p_sc_list, scp, scp_link);
	}

	bit = schedctl_alloc_bit(scp);
	PROC_UNLOCK(p);

	KASSERT(bit != -1, ("schedctl_alloc_bit returns invalid index"));
	*ssp = scp->scp_kaddr + bit;
	*uaddr = (caddr_t)(scp->scp_uaddr + bit);
	return (0);
}

void
schedctl_thread_exit(struct thread *td)
{
	sc_shared_t	*ssp = td->td_schedctl;
	sc_page_t 	*scp;
	int		index = 0;
	
	PROC_LOCK_ASSERT(td->td_proc, MA_OWNED);
	if (ssp == NULL)
		return;
	thread_lock(td);
	ssp->sc_state = SC_FREE;
	td->td_schedctl = NULL;
	td->td_sc_uaddr = 0;
	thread_unlock(td);
	scp = schedctl_find_page(ssp, &index);
	KASSERT(scp != NULL, ("failed to find sc_page"));
	schedctl_clear_bit(scp, index);
}

static void
schedctl_proc_ctor(void *arg, struct proc *p)
{
	SLIST_INIT(&p->p_sc_list);
}

static void
schedctl_proc_cleanup(void *arg)
{
	struct thread	*td = curthread;
	struct proc	*p = td->td_proc;
	sc_page_t	*scp;

	PROC_LOCK(p);
	schedctl_thread_exit(td);
	PROC_UNLOCK(p);

	/*
	 * Locking is unnecessary, because the process is already
	 * single-threaded.
	 */
	while ((scp = SLIST_FIRST(&p->p_sc_list)) != NULL) {
		SLIST_REMOVE_HEAD(&p->p_sc_list, scp_link);
		schedctl_free_page(scp);
	}
}

static int
schedctl_alloc_page(sc_page_t **scpp)
{
	sc_page_t	*scp;
	vm_map_t	map;
	vm_offset_t	addr;
	pmap_t		pmap;
	int		error;
	int		i;

	map = &curproc->p_vmspace->vm_map;
	pmap = vmspace_pmap(curproc->p_vmspace);
	scp = (sc_page_t *)uma_zalloc(sc_page_zone, M_WAITOK|M_ZERO);

	error = vm_mmap(map, &addr, PAGE_SIZE, VM_PROT_ALL, VM_PROT_ALL,
		MAP_ANON, OBJT_DEFAULT, NULL, 0);

	if (error) {
		uma_zfree(sc_page_zone, scp);
		return (error);
	}

	scp->scp_uaddr = (sc_shared_t *)addr;
	while (scp->scp_page == NULL) {
		if (vm_fault_quick((caddr_t)addr, VM_PROT_WRITE) < 0) {

			uma_zfree(sc_page_zone, scp);

			vm_map_lock(map);
			vm_map_delete(map, addr, addr + PAGE_SIZE);
			vm_map_unlock(map);
			return (EFAULT);
		}
		scp->scp_page = pmap_extract_and_hold(pmap, addr, VM_PROT_WRITE);
	}

	addr = kmem_alloc_nofault(kernel_map, PAGE_SIZE);
	if (addr == 0) {
		vm_page_lock_queues();
		vm_page_unhold(scp->scp_page);
		vm_page_unlock_queues();

		uma_zfree(sc_page_zone, scp);

		vm_map_lock(map);
		vm_map_delete(map, addr, addr + PAGE_SIZE);
		vm_map_unlock(map);
		return (ENOMEM);
	}

	scp->scp_kaddr = (sc_shared_t *)addr;
	for (i = 0; i < _NSC_WORDS; ++i)
		scp->scp_bmp[i] = -1L;
	scp->scp_free = _NSC_PER_PAGE;

	pmap_qenter(addr, &scp->scp_page, 1);
	*scpp = scp;

	return (0);
}

static void
schedctl_free_page(sc_page_t *scp)
{
	vm_map_t	map;
	vm_offset_t	addr;

	map = &curproc->p_vmspace->vm_map;

	addr = (vm_offset_t)scp->scp_uaddr;
	vm_map_lock(map);
	vm_map_delete(map, addr, addr + PAGE_SIZE);
	vm_map_unlock(map);

	addr = (vm_offset_t)scp->scp_kaddr;
	kmem_free(kernel_map, addr, addr + PAGE_SIZE);

	vm_page_lock_queues();
	vm_page_unhold(scp->scp_page);
	vm_page_unlock_queues();

	uma_zfree(sc_page_zone, scp);
}

static int
schedctl_alloc_bit(sc_page_t *scp)
{
	long	v;
	long	lowest;
	int 	i;

	for (i = 0; i < _NSC_WORDS; ++i) {
		if ((v = scp->scp_bmp[i]) == 0)
			continue;
		lowest = (v & -v) - 1;
		scp->scp_bmp[i] &= ~(1L << lowest);
		scp->scp_free--;
		return (int)(i * _NSC_BITS + lowest);
	}

	return (-1);
}

static sc_page_t *
schedctl_find_page(sc_shared_t *ssp, int *index)
{
	struct proc *p = curproc;
	sc_page_t *scp;

	PROC_LOCK_ASSERT(p, MA_OWNED);
	SLIST_FOREACH(scp, &p->p_sc_list, scp_link) {
		if (ssp >= scp->scp_kaddr && 
		    ssp < scp->scp_kaddr + _NSC_PER_PAGE) {
			*index = (int)(ssp - scp->scp_kaddr);
			break;
		}
	}
	return (scp);
}

static void
schedctl_clear_bit(sc_page_t *scp, int index)
{
	scp->scp_bmp[index / _NSC_BITS] |= (1L << (index % _NSC_BITS));
	scp->scp_free++;
}