int bitSInit(BitStream *bs)
{
#define g_b(d_,p_) (((d_)>>((p_)%8))&1)
#define s_b(d_,p_,v_) (d_)=((d_)&(~(1<<((p_)%8))))|((!!(v_))<<((p_)%8))
#define set_cr(n_,b_) s_b(bs->cregs[3-(n_)/8],n_,b_)
#define get_cr(n_) g_b(bs->cregs[3-(n_)/8],n_)
bs->nu=0;
bs->pe=0xb7;
bs->po=0xe7;
memset(bs->cregs,0,sizeof(bs->cregs));
set_cr(3,1);//check possible
set_cr(31,1);//IO tristate when dl
dlistInit(&bs->windows);
return 0;
}
void mem_initialize(void)
{
u32 cr;
u32 cookie = irq_kill();
gecko_printf("MEM: cleaning up\n");
_ic_inval();
_dc_inval();
_tlb_inval();
gecko_printf("MEM: unprotecting memory\n");
mem_protect(0,NULL,NULL);
gecko_printf("MEM: mapping sections\n");
memset32(__page_table, 0, 16384);
map_section(0x000, 0x000, 0x018, WRITEBACK_CACHE | DOMAIN(0) | AP_RWUSER);
map_section(0x100, 0x100, 0x040, WRITEBACK_CACHE | DOMAIN(0) | AP_RWUSER);
map_section(0x0d0, 0x0d0, 0x001, NONBUFFERABLE | DOMAIN(0) | AP_RWUSER);
map_section(0x0d8, 0x0d8, 0x001, NONBUFFERABLE | DOMAIN(0) | AP_RWUSER);
map_section(0xfff, 0xfff, 0x001, WRITEBACK_CACHE | DOMAIN(0) | AP_RWUSER);
set_dacr(0xFFFFFFFF); //manager access for all domains, ignore AP
set_ttbr((u32)__page_table); //configure translation table
_drain_write_buffer();
cr = get_cr();
#ifndef NO_CACHES
gecko_printf("MEM: enabling caches\n");
cr |= CR_DCACHE | CR_ICACHE;
set_cr(cr);
gecko_printf("MEM: enabling MMU\n");
cr |= CR_MMU;
set_cr(cr);
#endif
gecko_printf("MEM: init done\n");
irq_restore(cookie);
}
void exception_initialize(void)
{
exc_setup_stack();
u32 cr = get_cr();
cr |= 0x2; // Data alignment fault checking enable
set_cr(cr);
}
void PowerON_Reset_PC(void)
{
/* ==== VBR setting ==== */
set_vbr((void *)((char *)&INT_Vectors - INT_OFFSET));
/* ==== HardwareSetup Function ==== */
HardwareSetup(); /* Use Hardware Setup */
/* ==== B and D sections initialization ==== */
#ifdef COPY_SECTION_P
ProgCopy();
#endif
_INITSCT();
/* ==== Status Register setting ==== */
set_cr(SR_Init);
/* ==== Main function call ==== */
main();
/* ==== Sleep instruction execute ==== */
sleep();
}
static void sh7372_enter_core_standby(void)
{
void __iomem *smfram = (void __iomem *)SMFRAM;
__raw_writel(0, APARMBAREA); /* translate 4k */
__raw_writel(__pa(sh7372_cpu_resume), SBAR); /* set reset vector */
__raw_writel(0x10, SYSTBCR); /* enable core standby */
__raw_writel(0, smfram + 0x3c); /* clear page table address */
sh7372_cpu_suspend();
cpu_init();
/* if page table address is non-NULL then we have been powered down */
if (__raw_readl(smfram + 0x3c)) {
__raw_writel(__raw_readl(smfram + 0x40),
__va(__raw_readl(smfram + 0x3c)));
flush_tlb_all();
set_cr(__raw_readl(smfram + 0x38));
}
__raw_writel(0, SYSTBCR); /* disable core standby */
__raw_writel(0, SBAR); /* disable reset vector translation */
}
static int __init noalign_setup(char *__unused)
{
cr_alignment &= ~CR_A;
cr_no_alignment &= ~CR_A;
set_cr(cr_alignment);
return 1;
}
static int nommu_v7_vectors_init(void)
{
void *vectors;
u32 cr;
if (cpu_architecture() < CPU_ARCH_ARMv7)
return 0;
/*
* High vectors cannot be re-mapped, so we have to use normal
* vectors
*/
cr = get_cr();
cr &= ~CR_V;
set_cr(cr);
arm_fixup_vectors();
vectors = xmemalign(PAGE_SIZE, PAGE_SIZE);
memset(vectors, 0, PAGE_SIZE);
memcpy(vectors, __exceptions_start, __exceptions_size);
set_vbar((unsigned int)vectors);
return 0;
}
void PowerON_Reset_PC(void)
{
char *p;
set_vbr(&INT_Vectors[-4]); // VBRの設定
set_fpscr(FPSCR_Init);
// _INITSCT(); // Rセクションの初期化(D→R)とBセクションのクリア (RAM上ロードのため未使用)
for (p = (char*)__sectop("B"); p < (char*)__secend("B"); p++) *p = 0; // Bセクションのクリア
// _CALL_INIT(); // Remove the comment when you use global class object
// _INIT_IOLIB(); // Enable I/O in the application(both SIM I/O and hardware I/O)
// errno=0; // Remove the comment when you use errno
// srand((_UINT)1); // Remove the comment when you use rand()
// _s1ptr=NULL; // Remove the comment when you use strtok()
// HardwareSetup(); // Use Hardware Setup
set_cr(SR_Init);
main();
// _CLOSEALL(); // Close I/O in the application(both SIM I/O andhardware I/O)
// _CALL_END(); // Remove the comment when you use global class object
sleep();
}
void PowerON_Reset_PC(void)
{
set_vbr((void *)((_UBYTE *)&INT_Vectors - INT_OFFSET));
// HardwareSetup(); // Use Hardware Setup
_INITSCT();
// _CALL_INIT(); // Remove the comment when you use global class object
// _INIT_IOLIB(); // Enable I/O in the application(both SIM I/O and hardware I/O)
// errno=0; // Remove the comment when you use errno
// srand((_UINT)1); // Remove the comment when you use rand()
// _s1ptr=NULL; // Remove the comment when you use strtok()
set_cr(SR_Init);
main();
// _CLOSEALL(); // Close I/O in the application(both SIM I/O andhardware I/O)
// _CALL_END(); // Remove the comment when you use global class object
while(1);
}
static int __init noalign_setup(char *__unused)
{
printk("noalign_setup\r\n");
cr_alignment &= ~2;
cr_no_alignment &= ~2;
set_cr(cr_alignment);
return 1;
}
/**
* Enable processor's instruction cache
*/
void icache_enable(void)
{
u32 r;
r = get_cr();
r |= CR_I;
set_cr(r);
}
static int __init nowrite_setup(char *__unused)
{
cr_alignment &= ~(8|4);
cr_no_alignment &= ~(8|4);
flush_cache_all();
set_cr(cr_alignment);
return 1;
}
/**
* Disable processor's instruction cache
*/
void icache_disable(void)
{
u32 r;
r = get_cr();
r &= ~CR_I;
set_cr(r);
}
/*
* These are useful for identifing cache coherency
* problems by allowing the cache or the cache and
* writebuffer to be turned off. (Note: the write
* buffer should not be on and the cache off).
*/
static int __init nocache_setup(char *__unused)
{
printk("nocache_setup\r\n");
cr_alignment &= ~4;
cr_no_alignment &= ~4;
flush_cache_all();
set_cr(cr_alignment);
return 1;
}
/*
* Set/clear program flow prediction and return the previous state.
*/
static int config_branch_prediction(int set_cr_z)
{
unsigned int cr;
/* System Control Register: 11th bit Z Branch prediction enable */
cr = get_cr();
set_cr(set_cr_z ? cr | CR_Z : cr & ~CR_Z);
return cr & CR_Z;
}
void mem_shutdown(void)
{
u32 cookie = irq_kill();
_dc_flush();
_drain_write_buffer();
u32 cr = get_cr();
cr &= ~(CR_MMU | CR_DCACHE | CR_ICACHE); //disable ICACHE, DCACHE, MMU
set_cr(cr);
_ic_inval();
_dc_inval();
_tlb_inval();
irq_restore(cookie);
}
void mmu_early_disable(void)
{
unsigned int cr;
cr = get_cr();
cr &= ~(CR_M | CR_C);
set_cr(cr);
v8_flush_dcache_all();
tlb_invalidate();
dsb();
isb();
}
void adjust_cr(unsigned long mask, unsigned long set)
{
unsigned long flags;
mask &= ~CR_A;
set &= mask;
local_irq_save(flags);
cr_no_alignment = (cr_no_alignment & ~mask) | set;
cr_alignment = (cr_alignment & ~mask) | set;
set_cr((get_cr() & ~mask) | set);
local_irq_restore(flags);
}
// Execute EMUL_OP routine
void sheepshaver_cpu::execute_emul_op(uint32 emul_op)
{
M68kRegisters r68;
WriteMacInt32(XLM_68K_R25, gpr(25));
WriteMacInt32(XLM_RUN_MODE, MODE_EMUL_OP);
for (int i = 0; i < 8; i++)
r68.d[i] = gpr(8 + i);
for (int i = 0; i < 7; i++)
r68.a[i] = gpr(16 + i);
r68.a[7] = gpr(1);
uint32 saved_cr = get_cr() & 0xff9fffff; // mask_operand::compute(11, 8)
uint32 saved_xer = get_xer();
EmulOp(&r68, gpr(24), emul_op);
set_cr(saved_cr);
set_xer(saved_xer);
for (int i = 0; i < 8; i++)
gpr(8 + i) = r68.d[i];
for (int i = 0; i < 7; i++)
gpr(16 + i) = r68.a[i];
gpr(1) = r68.a[7];
WriteMacInt32(XLM_RUN_MODE, MODE_68K);
}
static void tc2_pm_psci_power_down(void)
{
struct psci_power_state power_state;
unsigned int mpidr, cpu, cluster;
mpidr = read_cpuid_mpidr();
cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);
BUG_ON(!psci_ops.cpu_off);
switch (atomic_dec_return(&tc2_pm_use_count[cpu][cluster])) {
case 1:
/*
* Overtaken by a power up. Flush caches, exit coherency,
* return & fake a reset
*/
set_cr(get_cr() & ~CR_C);
flush_cache_louis();
asm volatile ("clrex");
set_auxcr(get_auxcr() & ~(1 << 6));
return;
case 0:
/* A normal request to possibly power down the cluster */
power_state.id = PSCI_POWER_STATE_ID;
power_state.type = PSCI_POWER_STATE_TYPE_POWER_DOWN;
power_state.affinity_level = PSCI_POWER_STATE_AFFINITY_LEVEL1;
psci_ops.cpu_off(power_state);
/* On success this function never returns */
default:
/* Any other value is a bug */
BUG();
}
}
void mmu_early_enable(unsigned long membase, unsigned long memsize,
unsigned long ttb)
{
int el;
/*
* For the early code we only create level 1 pagetables which only
* allow for a 1GiB granularity. If our membase is not aligned to that
* bail out without enabling the MMU.
*/
if (membase & ((1ULL << level2shift(1)) - 1))
return;
memset((void *)ttb, 0, GRANULE_SIZE);
el = current_el();
set_ttbr_tcr_mair(el, ttb, calc_tcr(el), MEMORY_ATTRIBUTES);
create_sections((void *)ttb, 0, 0, 1UL << (BITS_PER_VA - 1), UNCACHED_MEM);
create_sections((void *)ttb, membase, membase, memsize, CACHED_MEM);
tlb_invalidate();
isb();
set_cr(get_cr() | CR_M);
}
/*
* Function to restore the table entry that
* was modified for enabling MMU
*/
static void restore_mmu_table_entry(void)
{
u32 *scratchpad_address;
u32 previous_value, control_reg_value;
u32 *address;
/*
* Get address of entry that was modified
*/
scratchpad_address = sar_ram_base + MMU_OFFSET;
address = (u32 *)readl(scratchpad_address + TABLE_ADDRESS_OFFSET);
/*
* Get the previous value which needs to be restored
*/
previous_value = readl(scratchpad_address + TABLE_VALUE_OFFSET);
address = __va(address);
*address = previous_value;
flush_tlb_all();
control_reg_value = readl(scratchpad_address + CR_VALUE_OFFSET);
/*
* Restore the Control register
*/
set_cr(control_reg_value);
}
static int
do_alignment_exception(struct pt_regs *regs)
{
unsigned int instr, rd, rn, correction, nr_regs, regbits;
unsigned long eaddr;
union { unsigned long un; signed long sn; } offset;
if (user_mode(regs)) {
set_cr(cr_no_alignment);
ai_user += 1;
return 0;
}
ai_sys += 1;
instr = *(unsigned long *)instruction_pointer(regs);
correction = 4; /* sometimes 8 on ARMv3 */
regs->ARM_pc += correction + 4;
rd = RD_BITS(instr);
rn = RN_BITS(instr);
eaddr = regs->uregs[rn];
switch(CODING_BITS(instr)) {
case 0x00000000:
if ((instr & 0x0ff00ff0) == 0x01000090) {
ai_skipped += 1;
printk(KERN_ERR "Unaligned trap: not handling swp instruction\n");
return 1;
}
if (((instr & 0x0e000090) == 0x00000090) && (instr & 0x60) != 0) {
ai_half += 1;
if (LDSTH_I_BIT(instr))
offset.un = (instr & 0xf00) >> 4 | (instr & 15);
else
offset.un = regs->uregs[RM_BITS(instr)];
if (LDST_P_BIT(instr)) {
if (LDST_U_BIT(instr))
eaddr += offset.un;
else
eaddr -= offset.un;
}
if (LDST_L_BIT(instr))
regs->uregs[rd] = get_unaligned((unsigned short *)eaddr);
else
put_unaligned(regs->uregs[rd], (unsigned short *)eaddr);
/* signed half-word? */
if (instr & 0x40)
regs->uregs[rd] = (long)((short) regs->uregs[rd]);
if (!LDST_P_BIT(instr)) {
if (LDST_U_BIT(instr))
eaddr += offset.un;
else
eaddr -= offset.un;
regs->uregs[rn] = eaddr;
} else if (LDST_W_BIT(instr))
regs->uregs[rn] = eaddr;
break;
}
int bitSSCR(BitStream *bs,int n,int v)
{
assert(n>=0);
assert(n<=31);
return set_cr(n,v);
}