static void
vlog(int pri, const char *token, const char *fmt, va_list ap)
{
if (logh) {
char *result;
if (vasprintf(&result, fmt, ap) != -1) {
logh(pri, result, logh_arg);
free(result);
return;
}
/* Otherwise, fallback to output on stderr. */
}
if (debug || logh) {
char *nfmt;
/* best effort in out of mem situations */
if (asprintf(&nfmt, "%s %s%s%s]%s %s\n",
date(),
translate(STDERR_FILENO, pri),
token ? "/" : "", token ? token : "",
isatty(STDERR_FILENO) ? "\033[0m" : "",
fmt) == -1) {
vfprintf(stderr, fmt, ap);
fprintf(stderr, "\n");
} else {
vfprintf(stderr, nfmt, ap);
free(nfmt);
}
fflush(stderr);
} else
vsyslog(pri, fmt, ap);
}
void hw_pi_dma(void)
{
if(WPI[3])
{
logh("hw: PI Dma Transfer (dram:%08X cart:%08X rd:%X wr:%X st:%X)\n",
WPI[0],WPI[1],WPI[2],WPI[3],WPI[4]);
osPiStartDma(0,0,0,WPI[1],WPI[0],WPI[3]+1,0);
WPI[3]=0;
}
}
void hw_si_pads(int write)
{
dword base=WSI[0];
int i;
logh("hw-si: dma %08X (write=%i)\n",base,write);
if(!cart.first_pad)
{
print("note: first pad access\n");
cart.first_pad=1;
}
if(write)
{
for(i=0;i<16;i++)
{
RPIF[0x1f0+i]=mem_read32(base+i*4);
}
}
else
{
// 2 dwords for each controller:
// 000000ss bbbbxxyy
// ss=status? !&00C0 or error
// bbbb=buttons
// xx=stick-x
// yy=stick-y
// construct reply
memset(RPIF+0x1f0,0,16*4);
RPIF[0x1f0+1+2*selectpad]=pad_getdata(0);
// copy it
for(i=0;i<16;i++)
{
mem_write32(base+i*4,RPIF[0x1f0+i]);
}
}
/*
for(i=0;i<16;i++)
{
print("%08X ",RPIF[0x1f0+i]);
if((i&3)==3) print("\n");
}
*/
WSI[1]=NULLFILL;
WSI[4]=NULLFILL;
RSI[0]=WSI[0];
RSI[6]=0; // not busy
os_event(OS_EVENT_SI);
}
void hw_sp_start(void)
{
logh("hw-sp: task %08X (type=%04X,flag=%04X,ucode=%08X/%04X,data=%08X/%04X,boot=%08X/%04X,yield=%08X/%04X)\n",
sptaskpos,
sptask.type,sptask.flags,
sptask.m_ucode,sptask.ucode_size,
sptask.m_data_ptr,sptask.data_size,
sptask.m_ucode_boot,sptask.ucode_boot_size,
sptask.m_yield_data_ptr,sptask.yield_data_size);
if(sptask.type==1)
{
// GFX task
if(sptask.flags&1)
{
// ignore yield continues, we have all the
// info from the first exec. Keep spexecuting=1
// since we are still processing
return;
}
else
{
memcpy(&st2.gfxtask,&sptask,sizeof(OSTask_t));
hw_sp_startgfx();
}
}
else
{
// NON-GFX task
if(sptask.type==2)
{
memcpy(&st2.audiotask,&sptask,sizeof(OSTask_t));
if(st.soundenable) slist_execute(&st2.audiotask);
os_event(OS_EVENT_SP);
}
else if(sptask.type==4 && cart.iszelda)
{
zlist_uncompress(&sptask);
os_event(OS_EVENT_SP); // sp task done
}
else
{
error("Unknown SP task type %04X\n",sptask.type);
os_event(OS_EVENT_SP); // sp task done
}
}
sploaded=0;
}
Heap::HeapEntry *Heap::allocFromFree(size_t _cbSize)
{
size_t lg = logh(_cbSize);
size_t div = ((1 << lg) / HEAP_ALLOC_SLOTS);
size_t slot = 0;
if (div == 0)
{
slot = 0;
}
else
{
slot = (_cbSize - (1 << lg)) / div;
}
for (; lg < HEAP_ALLOC_BITS; lg ++)
{
for (; slot < HEAP_ALLOC_SLOTS; slot ++)
{
HeapEntry *entry = m_free[lg][slot];
HeapEntry *prev = NULL;
while (entry)
{
if (entry->cbSize >= _cbSize)
{
if (prev)
{
prev->pNext = entry->pNext;
}
else
{
m_free[lg][slot] = entry->pNext;
}
assert (entry->cbSize >= _cbSize);
return entry;
}
prev = entry;
entry = entry->pNext;
}
}
slot = 0;
}
return NULL;
}
void Heap::free(void *ptr)
{
HeapEntry *freeEntry = (((HeapEntry *) ptr) - 1);
size_t index = logh(freeEntry->cbSize);
assert (index < HEAP_ALLOC_BITS);
size_t div = (1 << index) / HEAP_ALLOC_SLOTS;
size_t slot = 0;
if (div == 0)
{
slot = 0;
}
else
{
slot = (freeEntry->cbSize - (1 << index)) / div;
}
freeEntry->pNext = m_free[index][slot];
m_free[index][slot] = freeEntry;
this->m_cbAllocSize -= freeEntry->cbSize;
}
SEXP hitrun(SEXP alpha, SEXP initial, SEXP nbatch, SEXP blen, SEXP nspac,
SEXP origin, SEXP basis, SEXP amat, SEXP bvec, SEXP outmat, SEXP debug)
{
if (! isReal(alpha))
error("argument \"alpha\" must be type double");
if (! isReal(initial))
error("argument \"initial\" must be type double");
if (! isInteger(nbatch))
error("argument \"nbatch\" must be type integer");
if (! isInteger(blen))
error("argument \"blen\" must be type integer");
if (! isInteger(nspac))
error("argument \"nspac\" must be type integer");
if (! isReal(origin))
error("argument \"origin\" must be type double");
if (! isReal(basis))
error("argument \"basis\" must be type double");
if (! isReal(amat))
error("argument \"amat\" must be type double");
if (! isReal(bvec))
error("argument \"bvec\" must be type double");
if (! (isNull(outmat) | isReal(outmat)))
error("argument \"outmat\" must be type double or NULL");
if (! isLogical(debug))
error("argument \"debug\" must be logical");
if (! isMatrix(basis))
error("argument \"basis\" must be matrix");
if (! isMatrix(amat))
error("argument \"amat\" must be matrix");
if (! (isNull(outmat) | isMatrix(outmat)))
error("argument \"outmat\" must be matrix or NULL");
int dim_oc = LENGTH(alpha);
int dim_nc = LENGTH(initial);
int ncons = nrows(amat);
if (LENGTH(nbatch) != 1)
error("argument \"nbatch\" must be scalar");
if (LENGTH(blen) != 1)
error("argument \"blen\" must be scalar");
if (LENGTH(nspac) != 1)
error("argument \"nspac\" must be scalar");
if (LENGTH(origin) != dim_oc)
error("length(origin) != length(alpha)");
if (nrows(basis) != dim_oc)
error("nrow(basis) != length(alpha)");
if (ncols(basis) != dim_nc)
error("ncol(basis) != length(initial)");
if (ncols(amat) != dim_nc)
error("ncol(amat) != length(initial)");
if (LENGTH(bvec) != ncons)
error("length(bvec) != nrow(amat)");
if (LENGTH(debug) != 1)
error("argument \"debug\" must be scalar");
int dim_out = dim_oc;
if (! isNull(outmat)) {
dim_out = nrows(outmat);
if (ncols(outmat) != dim_oc)
error("ncol(outmat) != length(alpha)");
}
int int_nbatch = INTEGER(nbatch)[0];
int int_blen = INTEGER(blen)[0];
int int_nspac = INTEGER(nspac)[0];
int int_debug = LOGICAL(debug)[0];
double *dbl_star_alpha = REAL(alpha);
double *dbl_star_initial = REAL(initial);
double *dbl_star_origin = REAL(origin);
double *dbl_star_basis = REAL(basis);
double *dbl_star_amat = REAL(amat);
double *dbl_star_bvec = REAL(bvec);
int has_outmat = isMatrix(outmat);
double *dbl_star_outmat = 0;
if (has_outmat)
dbl_star_outmat = REAL(outmat);
if (int_nbatch <= 0)
error("argument \"nbatch\" must be positive");
if (int_blen <= 0)
error("argument \"blen\" must be positive");
if (int_nspac <= 0)
error("argument \"nspac\" must be positive");
check_finite(dbl_star_alpha, dim_oc, "alpha");
check_positive(dbl_star_alpha, dim_oc, "alpha");
check_finite(dbl_star_initial, dim_nc, "initial");
check_finite(dbl_star_origin, dim_oc, "origin");
check_finite(dbl_star_basis, dim_oc * dim_nc, "basis");
check_finite(dbl_star_amat, ncons * dim_nc, "amat");
check_finite(dbl_star_bvec, ncons, "bvec");
if (has_outmat)
check_finite(dbl_star_outmat, dim_out * dim_oc, "outmat");
double *state = (double *) R_alloc(dim_nc, sizeof(double));
double *proposal = (double *) R_alloc(dim_nc, sizeof(double));
double *batch_buffer = (double *) R_alloc(dim_out, sizeof(double));
double *out_buffer = (double *) R_alloc(dim_out, sizeof(double));
memcpy(state, dbl_star_initial, dim_nc * sizeof(double));
logh_setup(dbl_star_alpha, dbl_star_origin, dbl_star_basis, dim_oc, dim_nc);
double current_log_dens = logh(state);
out_setup(dbl_star_origin, dbl_star_basis, dbl_star_outmat, dim_oc, dim_nc,
dim_out, has_outmat);
SEXP result, resultnames, path, save_initial, save_final;
if (! int_debug) {
PROTECT(result = allocVector(VECSXP, 3));
PROTECT(resultnames = allocVector(STRSXP, 3));
} else {
PROTECT(result = allocVector(VECSXP, 11));
PROTECT(resultnames = allocVector(STRSXP, 11));
}
PROTECT(path = allocMatrix(REALSXP, dim_out, int_nbatch));
SET_VECTOR_ELT(result, 0, path);
PROTECT(save_initial = duplicate(initial));
SET_VECTOR_ELT(result, 1, save_initial);
UNPROTECT(2);
SET_STRING_ELT(resultnames, 0, mkChar("batch"));
SET_STRING_ELT(resultnames, 1, mkChar("initial"));
SET_STRING_ELT(resultnames, 2, mkChar("final"));
if (int_debug) {
SEXP spath, ppath, zpath, u1path, u2path, s1path, s2path, gpath;
int nn = int_nbatch * int_blen * int_nspac;
PROTECT(spath = allocMatrix(REALSXP, dim_nc, nn));
SET_VECTOR_ELT(result, 3, spath);
PROTECT(ppath = allocMatrix(REALSXP, dim_nc, nn));
SET_VECTOR_ELT(result, 4, ppath);
PROTECT(zpath = allocMatrix(REALSXP, dim_nc, nn));
SET_VECTOR_ELT(result, 5, zpath);
PROTECT(u1path = allocVector(REALSXP, nn));
SET_VECTOR_ELT(result, 6, u1path);
PROTECT(u2path = allocVector(REALSXP, nn));
SET_VECTOR_ELT(result, 7, u2path);
PROTECT(s1path = allocVector(REALSXP, nn));
SET_VECTOR_ELT(result, 8, s1path);
PROTECT(s2path = allocVector(REALSXP, nn));
SET_VECTOR_ELT(result, 9, s2path);
PROTECT(gpath = allocVector(REALSXP, nn));
SET_VECTOR_ELT(result, 10, gpath);
UNPROTECT(8);
SET_STRING_ELT(resultnames, 3, mkChar("current"));
SET_STRING_ELT(resultnames, 4, mkChar("proposal"));
SET_STRING_ELT(resultnames, 5, mkChar("z"));
SET_STRING_ELT(resultnames, 6, mkChar("u1"));
SET_STRING_ELT(resultnames, 7, mkChar("u2"));
SET_STRING_ELT(resultnames, 8, mkChar("s1"));
SET_STRING_ELT(resultnames, 9, mkChar("s2"));
SET_STRING_ELT(resultnames, 10, mkChar("log.green"));
}
namesgets(result, resultnames);
UNPROTECT(1);
GetRNGstate();
if (current_log_dens == R_NegInf)
error("log unnormalized density -Inf at initial state");
for (int ibatch = 0, k = 0; ibatch < int_nbatch; ibatch++) {
for (int i = 0; i < dim_out; i++)
batch_buffer[i] = 0.0;
for (int jbatch = 0; jbatch < int_blen; jbatch++) {
double proposal_log_dens;
for (int ispac = 0; ispac < int_nspac; ispac++) {
/* Note: should never happen! */
if (current_log_dens == R_NegInf)
error("log density -Inf at current state");
double u1 = R_NaReal;
double u2 = R_NaReal;
double smax = R_NaReal;
double smin = R_NaReal;
double z[dim_nc];
propose(state, proposal, dbl_star_amat, dbl_star_bvec,
dim_nc, ncons, z, &smax, &smin, &u1);
proposal_log_dens = logh(proposal);
int accept = FALSE;
if (proposal_log_dens != R_NegInf) {
if (proposal_log_dens >= current_log_dens) {
accept = TRUE;
} else {
double green = exp(proposal_log_dens
- current_log_dens);
u2 = unif_rand();
accept = u2 < green;
}
}
if (int_debug) {
int l = ispac + int_nspac * (jbatch + int_blen * ibatch);
int lbase = l * dim_nc;
SEXP spath = VECTOR_ELT(result, 3);
SEXP ppath = VECTOR_ELT(result, 4);
SEXP zpath = VECTOR_ELT(result, 5);
SEXP u1path = VECTOR_ELT(result, 6);
SEXP u2path = VECTOR_ELT(result, 7);
SEXP s1path = VECTOR_ELT(result, 8);
SEXP s2path = VECTOR_ELT(result, 9);
SEXP gpath = VECTOR_ELT(result, 10);
for (int lj = 0; lj < dim_nc; lj++) {
REAL(spath)[lbase + lj] = state[lj];
REAL(ppath)[lbase + lj] = proposal[lj];
REAL(zpath)[lbase + lj] = z[lj];
}
REAL(u1path)[l] = u1;
REAL(u2path)[l] = u2;
REAL(s1path)[l] = smin;
REAL(s2path)[l] = smax;
REAL(gpath)[l] = proposal_log_dens - current_log_dens;
}
if (accept) {
memcpy(state, proposal, dim_nc * sizeof(double));
current_log_dens = proposal_log_dens;
}
} /* end of inner loop (one iteration) */
outfun(state, out_buffer);
for (int j = 0; j < dim_out; j++)
batch_buffer[j] += out_buffer[j];
} /* end of middle loop (one batch) */
for (int j = 0; j < dim_out; j++, k++)
REAL(path)[k] = batch_buffer[j] / int_blen;
} /* end of outer loop */
PutRNGstate();
PROTECT(save_final = allocVector(REALSXP, dim_nc));
memcpy(REAL(save_final), state, dim_nc * sizeof(double));
SET_VECTOR_ELT(result, 2, save_final);
UNPROTECT(5);
return result;
}
