/* Starting another CPUs */
__externC void cyg_hal_cpu_start(HAL_SMP_CPU_TYPE cpu) {
if (cyg_hal_smp_cpu_running[cpu] == 1)
return;
if (cpu == 0) {
cyg_hal_smp_cpu_running[cpu] = 1;
hal_scu_join_smp();
} else {
hal_delay_us(100);
/* Flush cache */
HAL_DCACHE_INVALIDATE_ALL();
HAL_ICACHE_INVALIDATE_ALL();
HAL_DCACHE_SYNC();
HAL_ICACHE_SYNC();
zynq_cpu_stop(cpu);
cyg_uint32 trampoline_size =
(cyg_uint32)&zynq_secondary_trampoline_jump -
(cyg_uint32)&zynq_secondary_trampoline;
memcpy(0x0, &zynq_secondary_trampoline, trampoline_size);
HAL_WRITE_UINT32(0x0 + trampoline_size,
(cyg_uint32)&cyg_hal_smp_start_secondary_cpu);
zynq_cpu_start(cpu);
}
}
void
__install_breakpoint_list (void)
{
struct breakpoint_list *l = breakpoint_list;
while (l != NULL)
{
if (! l->in_memory)
{
int len = sizeof (l->old_contents);
if (__read_mem_safe (&l->old_contents[0], (void*)l->addr, len) == len)
{
if (__write_mem_safe (HAL_BREAKINST_ADDR(l->length),
(void*)l->addr, l->length) == l->length)
{
l->in_memory = 1;
}
}
}
l = l->next;
}
#if defined(HAL_STUB_HW_BREAKPOINT_LIST_SIZE) && (HAL_STUB_HW_BREAKPOINT_LIST_SIZE > 0)
__install_hw_breakpoint_list();
#endif
#if defined(HAL_STUB_HW_WATCHPOINT_LIST_SIZE) && (HAL_STUB_HW_WATCHPOINT_LIST_SIZE > 0)
__install_hw_watchpoint_list();
#endif
HAL_ICACHE_SYNC();
}
static inline cyg_uint32 *pci_config_setup(cyg_uint32 bus,
cyg_uint32 devfn,
cyg_uint32 offset)
{
cyg_uint32 *pdata, *paddr;
cyg_uint32 dev = CYG_PCI_DEV_GET_DEV(devfn);
cyg_uint32 fn = CYG_PCI_DEV_GET_FN(devfn);
if (bus == 0) {
paddr = (cyg_uint32 *)POCCAR_ADDR;
pdata = (cyg_uint32 *)POCCDR_ADDR;
} else {
paddr = (cyg_uint32 *)SOCCAR_ADDR;
pdata = (cyg_uint32 *)SOCCDR_ADDR;
}
/* Offsets must be dword-aligned */
offset &= ~3;
/* Primary or secondary bus use type 0 config */
/* all others use type 1 config */
if (bus == pbus_nr || bus == sbus_nr)
*paddr = ( (1 << (dev + 16)) | (fn << 8) | offset | 0 );
else
*paddr = ( (bus << 16) | (dev << 11) | (fn << 8) | offset | 1 );
orig_abort_vec = ((volatile cyg_uint32 *)0x20)[4];
((volatile unsigned *)0x20)[4] = (unsigned)__pci_abort_handler;
HAL_ICACHE_SYNC();
return pdata;
}
void
install_async_breakpoint(void *epc)
{
CYGARC_HAL_SAVE_GP();
asyncBuffer.targetAddr = epc;
asyncBuffer.savedInstr = *(t_inst *)epc;
*(t_inst *)epc = *(t_inst *)_breakinst;
HAL_DCACHE_SYNC();
HAL_ICACHE_SYNC();
CYGARC_HAL_RESTORE_GP();
}
static inline int pci_config_cleanup(cyg_uint32 bus)
{
cyg_uint32 status = 0, err = 0;
if (bus == pbus_nr) {
status = *(cyg_uint16 *) PATUSR_ADDR;
if ((status & 0xF900) != 0) {
err = 1;
*(cyg_uint16 *)PATUSR_ADDR = status & 0xF980;
}
status = *(cyg_uint16 *) PSR_ADDR;
if ((status & 0xF900) != 0) {
err = 1;
*(cyg_uint16 *)PSR_ADDR = status & 0xF980;
}
status = *(cyg_uint32 *) PATUISR_ADDR;
if ((status & 0x79F) != 0) {
err = 1;
*(cyg_uint32 *) PATUISR_ADDR = status & 0x79f;
}
status = *(cyg_uint32 *) PBISR_ADDR;
if ((status & 0x3F) != 0) {
err = 1;
*(cyg_uint32 *) PBISR_ADDR = status & 0x3F;
}
} else {
status = *(cyg_uint16 *) SATUSR_ADDR;
if ((status & 0xF900) != 0) {
err = 1;
*(cyg_uint16 *) SATUSR_ADDR = status & 0xF900;
}
status = *(cyg_uint16 *) SSR_ADDR;
if ((status & 0xF900) != 0) {
err = 1;
*(cyg_uint16 *) SSR_ADDR = status & 0xF980;
}
status = *(cyg_uint32 *) SATUISR_ADDR;
if ((status & 0x69F) != 0) {
err = 1;
*(cyg_uint32 *) SATUISR_ADDR = status & 0x69F;
}
}
((volatile unsigned *)0x20)[4] = orig_abort_vec;
HAL_ICACHE_SYNC();
return err;
}
int __computeSignal (unsigned int trap_number)
{
// Treat everything as a break point
if (asyncBuffer.targetAddr != NULL)
{
// BP installed by serial driver to stop running program
*asyncBuffer.targetAddr = asyncBuffer.savedInstr;
HAL_DCACHE_SYNC();
HAL_ICACHE_SYNC();
asyncBuffer.targetAddr = NULL;
return SIGINT;
}
return SIGTRAP;
}
void __install_breakpoints ()
{
if (instrBuffer.targetAddr != NULL)
{
instrBuffer.savedInstr = *instrBuffer.targetAddr;
*instrBuffer.targetAddr = __break_opcode ();
}
// Ensure that any instructions that are about to be modified aren't in
// the instruction cache.
HAL_ICACHE_SYNC();
// Install the breakpoints in the breakpoint list
__install_breakpoint_list();
// Make sure the breakpoints have been written out to memory.
HAL_DCACHE_SYNC();
}
void
cyg_ldr_flush_cache(void)
{
HAL_DCACHE_SYNC();
HAL_ICACHE_SYNC();
}
