/*
 * A free operation has occurred -- update malloc type statistics for the
 * amount of the bucket size.  Occurs within a critical section so that the
 * thread isn't preempted and doesn't migrate while updating per-CPU
 * statistics.
 */
void
malloc_type_freed(struct malloc_type *mtp, unsigned long size)
{
	struct malloc_type_internal *mtip;
	struct malloc_type_stats *mtsp;

	critical_enter();
	mtip = mtp->ks_handle;
	mtsp = &mtip->mti_stats[curcpu];
	mtsp->mts_memfreed += size;
	mtsp->mts_numfrees++;

	critical_exit();
}

SpiFlashOpResult IRAM spi_flash_erase_sector(uint16_t sec)
{
    CHECK_PARAM_RET (sec < flashchip->chip_size / flashchip->sector_size, SPI_FLASH_RESULT_ERR);

    critical_enter ();
    Cache_Read_Disable();

    SpiFlashOpResult ret = SPIEraseSector (sec);

    Cache_Read_Enable(0, 0, 1);
    critical_exit ();

    return ret;
}

void
update_gdt_fsbase(struct thread *td, uint32_t base)
{
	struct user_segment_descriptor *sd;

	if (td != curthread)
		return;
	td->td_pcb->pcb_full_iret = 1;
	critical_enter();
	sd = PCPU_GET(fs32p);
	sd->sd_lobase = base & 0xffffff;
	sd->sd_hibase = (base >> 24) & 0xff;
	critical_exit();
}

/*
 * A very short dispatch, to try and maximise assembler code use
 * between all exception types. Maybe 'true' interrupts should go
 * here, and the trap code can come in separately
 */
void
powerpc_interrupt(struct trapframe *framep)
{
	struct thread *td;
	struct trapframe *oldframe;
	register_t ee;

	td = curthread;

	CTR2(KTR_INTR, "%s: EXC=%x", __func__, framep->exc);

	switch (framep->exc) {
	case EXC_EXI:
		critical_enter();
		PIC_DISPATCH(root_pic, framep);
		critical_exit();
		break;

	case EXC_DECR:
		critical_enter();
		atomic_add_int(&td->td_intr_nesting_level, 1);
		oldframe = td->td_intr_frame;
		td->td_intr_frame = framep;
		decr_intr(framep);
		td->td_intr_frame = oldframe;
		atomic_subtract_int(&td->td_intr_nesting_level, 1);
		critical_exit();
		break;

	default:
		/* Re-enable interrupts if applicable. */
		ee = framep->srr1 & PSL_EE;
		if (ee != 0)
			mtmsr(mfmsr() | ee);
		trap(framep);
	}	        
}

int
_rm_rlock_debug(struct rmlock *rm, struct rm_priotracker *tracker,
    int trylock, const char *file, int line)
{

	if (SCHEDULER_STOPPED())
		return (1);

#ifdef INVARIANTS
	if (!(rm->lock_object.lo_flags & LO_RECURSABLE) && !trylock) {
		critical_enter();
		KASSERT(rm_trackers_present(pcpu_find(curcpu), rm,
		    curthread) == 0,
		    ("rm_rlock: recursed on non-recursive rmlock %s @ %s:%d\n",
		    rm->lock_object.lo_name, file, line));
		critical_exit();
	}
#endif
	KASSERT(kdb_active != 0 || !TD_IS_IDLETHREAD(curthread),
	    ("rm_rlock() by idle thread %p on rmlock %s @ %s:%d",
	    curthread, rm->lock_object.lo_name, file, line));
	KASSERT(!rm_destroyed(rm),
	    ("rm_rlock() of destroyed rmlock @ %s:%d", file, line));
	if (!trylock) {
		KASSERT(!rm_wowned(rm),
		    ("rm_rlock: wlock already held for %s @ %s:%d",
		    rm->lock_object.lo_name, file, line));
		WITNESS_CHECKORDER(&rm->lock_object, LOP_NEWORDER, file, line,
		    NULL);
	}

	if (_rm_rlock(rm, tracker, trylock)) {
		if (trylock)
			LOCK_LOG_TRY("RMRLOCK", &rm->lock_object, 0, 1, file,
			    line);
		else
			LOCK_LOG_LOCK("RMRLOCK", &rm->lock_object, 0, 0, file,
			    line);
		WITNESS_LOCK(&rm->lock_object, 0, file, line);

		curthread->td_locks++;

		return (1);
	} else if (trylock)
		LOCK_LOG_TRY("RMRLOCK", &rm->lock_object, 0, 0, file, line);

	return (0);
}

setval_t set_lookup(set_t *s, setkey_t k)
{
    node_t *n;
    setval_t v = NULL;
    ptst_t *ptst;

    k = CALLER_TO_INTERNAL_KEY(k);

    ptst = critical_enter();

    n = weak_find(&s->root, k);
    if ( n != NULL ) v = GET_VALUE(n);

    critical_exit(ptst);
    return v;
}

t_max_err polywave_interp_set(t_polywave *x, t_object *attr, long argc, t_atom *argv)
{
    
    if(argc == 1 && atom_gettype(argv) == A_LONG)
    {
        critical_enter(x->lock);
        x->interp_type = atom_getlong(argv);
        x->backup = x->interp_type; // << HORRIBLE KLUDGE
        //post("%d %d", x->interp_type, x->backup);
        critical_exit(x->lock);
    } else {
        object_error((t_object *)x, "unknown interpolation ");
    }
    
    return 0;
}

int
cpu_set_user_tls(struct thread *td, void *tls_base)
{

	td->td_md.md_tp = (register_t)tls_base;
	if (td == curthread) {
		critical_enter();
#ifdef ARM_TP_ADDRESS
		*(register_t *)ARM_TP_ADDRESS = (register_t)tls_base;
#else
		set_tls((void *)tls_base);
#endif
		critical_exit();
	}
	return (0);
}

/*
 * Decrementer interrupt routine
 */
void
powerpc_decr_interrupt(struct trapframe *framep)
{
	struct thread *td;
	struct trapframe *oldframe;

	td = curthread;
	critical_enter();
	atomic_add_int(&td->td_intr_nesting_level, 1);
	oldframe = td->td_intr_frame;
	td->td_intr_frame = framep;
	decr_intr(framep);
	td->td_intr_frame = oldframe;
	atomic_subtract_int(&td->td_intr_nesting_level, 1);
	critical_exit();
	framep->srr1 &= ~PSL_WE;
}

void o_freenect_grab_callback(t_object *x_obj)
{
    t_jit_pcl_freenect *x = (t_jit_pcl_freenect *)x_obj;
    
    //    x->kinect->getRegisteredFrames();
    critical_enter(x->lock);
    
    if( x->kinect )
        x->kinect->getFrames( &x->rgb_frame, &x->depth_frame, &x->ir_frame );
    
    critical_exit(x->lock);
    
    
    if( x->rgb_frame )
        post("outer %ld %ld %d %d", x->rgb_frame->width, x->rgb_frame->height, x->rgb_frame->data[0], x->rgb_frame->data[1]);
    
}

void hub_init(t_hub *x, t_symbol*, long, t_atom*)
{
	subscriberList	*subscriber = x->subscriber;
	subscriberIterator i;

	// The flag is indicating that we're in the middle of initialization
	x->flag_init = 1;
	// Search the linked list for jcom.init objects and 'bang' them
	critical_enter(0);
	for(i = subscriber->begin(); i != subscriber->end(); ++i) {
		if((*i)->type == jps_subscribe_init)
			object_method((*i)->object, jps_go);		// TODO: This is an exceptionally bad thing to do inside of the critical region
														// It could result in a deadlock
	}
	
	critical_exit(0);
	defer_low(x, (method)hub_preset_default, 0, 0, 0L);
}

void
cpu_thread_exit(struct thread *td)
{
	struct pcb *pcb;

	critical_enter();
	if (td == PCPU_GET(fpcurthread))
		fpudrop();
	critical_exit();

	pcb = td->td_pcb;

	/* Disable any hardware breakpoints. */
	if (pcb->pcb_flags & PCB_DBREGS) {
		reset_dbregs();
		clear_pcb_flags(pcb, PCB_DBREGS);
	}
}

/*
 * call platform specific code to halt (until next interrupt) for the idle loop
 */
void
cpu_idle(int busy)
{
	KASSERT((mips_rd_status() & MIPS_SR_INT_IE) != 0,
		("interrupts disabled in idle process."));
	KASSERT((mips_rd_status() & MIPS_INT_MASK) != 0,
		("all interrupts masked in idle process."));

	if (!busy) {
		critical_enter();
		cpu_idleclock();
	}
	mips_wait();
	if (!busy) {
		cpu_activeclock();
		critical_exit();
	}
}

void hub_returnnames_get(t_hub *x)
{
	subscriberList	*subscriber = x->subscriber;	// head of the linked list
	t_atom			a;
	
	hub_outlet_return(x, jps_return_names_start, 0, NULL);
	
	subscriberIterator i;
	t_subscriber* t;
	critical_enter(0);
	for(i = subscriber->begin(); i != subscriber->end(); ++i) {
		t = *i;
		atom_setsym(&a, t->name);
		if(t->type == jps_subscribe_return)
			hub_outlet_return(x, jps_message_return, 1, &a);
	}
	critical_exit(0);
	hub_outlet_return(x, jps_return_names_end, 0, NULL);
}

/*
 * An allocation has succeeded -- update malloc type statistics for the
 * amount of bucket size.  Occurs within a critical section so that the
 * thread isn't preempted and doesn't migrate while updating per-PCU
 * statistics.
 */
static void
malloc_type_zone_allocated(struct malloc_type *mtp, unsigned long size,
    int zindx)
{
	struct malloc_type_internal *mtip;
	struct malloc_type_stats *mtsp;

	critical_enter();
	mtip = mtp->ks_handle;
	mtsp = &mtip->mti_stats[curcpu];
	if (size > 0) {
		mtsp->mts_memalloced += size;
		mtsp->mts_numallocs++;
	}
	if (zindx != -1)
		mtsp->mts_size |= 1 << zindx;

	critical_exit();
}

void o_freenect_grab_connect(t_jit_pcl_freenect *x)
{
    critical_enter(x->lock);
    
    x->kinect = new io_kinect_device;
    
    t_object *x_obj = (t_object *)x;
    std::function<void(t_object*)> cb = std::bind(o_freenect_grab_callback, x_obj);
    x->kinect->setCallbackType( cb, x_obj, libfreenect2::Frame::Type::Depth );
    
    if ( ! x->kinect->acitve() )
    {
        post("no active kinect");
        return;
    }
    
    critical_exit(x->lock);
    
}

void model_preset_dowrite(TTPtr self, t_symbol *msg, long argc, t_atom *argv)
{
	WrappedModularInstancePtr	x = (WrappedModularInstancePtr)self;
	char 			filename[MAX_FILENAME_CHARS];
	TTSymbol		fullpath;
	TTValue			o, v, none;
	TTObject        aTextHandler;
	TTErr			tterr;

	// stop filewatcher
	if (EXTRA->filewatcher)
		filewatcher_stop(EXTRA->filewatcher);

	if (EXTRA->presetManager->valid()) {

		// Default TEXT File Name
		snprintf(filename, MAX_FILENAME_CHARS, "%s.%s.presets.txt", x->patcherClass.c_str(), x->patcherContext.c_str());
		fullpath = jamoma_file_write((t_object*)x, argc, argv, filename);
		v.append(fullpath);

		tterr = x->internals->lookup(kTTSym_TextHandler, o);

		if (!tterr) {
			aTextHandler = o[0];

			aTextHandler.set(kTTSym_object, *EXTRA->presetManager);

			critical_enter(0);
			tterr = aTextHandler.send(kTTSym_Write, v, none);
			critical_exit(0);

			if (!tterr)
				object_obex_dumpout(self, _sym_write, argc, argv);
			else
				object_obex_dumpout(self, _sym_error, 0, NULL);
		}
	}

	// start filewatcher
	if (EXTRA->filewatcher)
		filewatcher_start(EXTRA->filewatcher);
}

static uintptr_t
unlock_rm(struct lock_object *lock)
{
	struct thread *td;
	struct pcpu *pc;
	struct rmlock *rm;
	struct rm_queue *queue;
	struct rm_priotracker *tracker;
	uintptr_t how;

	rm = (struct rmlock *)lock;
	tracker = NULL;
	how = 0;
	rm_assert(rm, RA_LOCKED | RA_NOTRECURSED);
	if (rm_wowned(rm))
		rm_wunlock(rm);
	else {
		/*
		 * Find the right rm_priotracker structure for curthread.
		 * The guarantee about its uniqueness is given by the fact
		 * we already asserted the lock wasn't recursively acquired.
		 */
		critical_enter();
		td = curthread;
		pc = pcpu_find(curcpu);
		for (queue = pc->pc_rm_queue.rmq_next;
		    queue != &pc->pc_rm_queue; queue = queue->rmq_next) {
			tracker = (struct rm_priotracker *)queue;
				if ((tracker->rmp_rmlock == rm) &&
				    (tracker->rmp_thread == td)) {
					how = (uintptr_t)tracker;
					break;
				}
		}
		KASSERT(tracker != NULL,
		    ("rm_priotracker is non-NULL when lock held in read mode"));
		critical_exit();
		rm_runlock(rm, tracker);
	}
	return (how);
}

setval_t set_lookup(set_t *s, setkey_t k)
{
    ptst_t  *ptst;
    node_t  *m, *n;
    setval_t v;

    k = CALLER_TO_INTERNAL_KEY(k);

    ptst = critical_enter();

    n = &s->root;
    while ( (m = (k <= n->k) ? n->l : n->r) != NULL )
        n = m;

    v = (k == n->k) ? GET_VALUE(n->v) : NULL;
    if ( v == GARBAGE_VALUE ) v = NULL;

    critical_exit(ptst);

    return v;
}

// FREEZE UI for all parameters
void hub_ui_freeze(t_hub *x, t_symbol*, long argc, t_atom *argv)
{
	subscriberList *subscriber = x->subscriber;	// head of the linked list
	t_max_err err = MAX_ERR_NONE;

	// Change freeze status for all messages and parameters
	subscriberIterator i;
	t_subscriber* t;

	// Change freeze attribute for the gui
	// FIXME: This call is not working!!!!
	// this means that the ui menu does not always reflect the state correctly
	err = object_attr_setlong(x->gui_object, gensym("ui_is_frozen"), atom_getlong(argv));

	critical_enter(0);
 	for(i = subscriber->begin(); i != subscriber->end(); ++i) {
		t = *i;
		if(t->type == jps_subscribe_parameter)
			object_method_typed(t->object, jps_ui_slash_freeze, 1, argv, NULL);
	}
	critical_exit(0);
}