pcmb_smp.c

开放源码实时操作系统源码.

{
    HAL_SMP_CPU_TYPE cpu;

//    PC_WRITE_SCREEN( PC_SCREEN_LINE(2)+0, '!' );

#ifndef CYG_HAL_STARTUP_RAM 
    // Wait for INIT interrupt to be deasserted
    while( !init_deasserted )
        continue;
#endif
    
//    PC_WRITE_SCREEN( PC_SCREEN_LINE(2)+1, '!' );
    
    cpu  = HAL_SMP_CPU_THIS();
    
//    PC_WRITE_SCREEN_8( PC_SCREEN_LINE(2)+6, cpu );
    
#ifndef CYG_HAL_STARTUP_RAM 
    // Wait 1s for the world to settle
    hal_delay_us( 1000000 );

//    PC_WRITE_SCREEN( PC_SCREEN_LINE(2)+2, '!' );        
 
    // Setup our APIC
    cyg_hal_smp_init_apic();
#endif
    
//    PC_WRITE_SCREEN( PC_SCREEN_LINE(2)+3, '!' );        

#ifdef CYGPKG_KERNEL_SMP_SUPPORT			
    cyg_hal_smp_cpu_running[cpu] = 1;
    cyg_kernel_smp_startup();
#else 
    for(;;)
    {
        void (*entry)(void);

        while( (entry = cyg_hal_smp_cpu_entry[cpu]) == 0 )
        {
#if 0 //SCREEN_DIAGNOSTICS                
            static int n;
            PC_WRITE_SCREEN_8( PC_SCREEN_LINE(2)+10, n );
            PC_WRITE_SCREEN_8( PC_SCREEN_LINE(2)+15, cyg_hal_smp_cpu_sync[cpu] );
            PC_WRITE_SCREEN_8( PC_SCREEN_LINE(2)+30, cyg_hal_smp_cpu_sync_flag[0] );
            PC_WRITE_SCREEN_8( PC_SCREEN_LINE(2)+35, cyg_hal_smp_cpu_sync_flag[1] );
            PC_WRITE_SCREEN_8( PC_SCREEN_LINE(2)+40, cyg_hal_smp_vsr_sync_flag );
            n++;
#endif
            hal_delay_us( 100 );            
        }

//        PC_WRITE_SCREEN( PC_SCREEN_LINE(2)+4, '!' );

        cyg_hal_smp_cpu_entry[cpu] = 0; 
 
//        PC_WRITE_SCREEN_32( PC_SCREEN_LINE(2)+20, entry );  
 
        if( entry != NULL )
        {
            cyg_hal_smp_cpu_running[cpu] = 1;
            entry();
        }
    }
#endif     
}

/*------------------------------------------------------------------------*/

__externC void cyg_hal_smp_init(void)
{
    if( !cyg_hal_find_smp_config() )
        return;

    if( !cyg_hal_parse_smp_config() )
        return;

    if( !cyg_hal_smp_init_apic() )
        return;

    if( !cyg_hal_smp_init_ioapic() )
        return;
}

/*------------------------------------------------------------------------*/

__externC void cyg_hal_smp_cpu_start_all(void)
{
    HAL_SMP_CPU_TYPE cpu;

    for( cpu = 0; cpu < HAL_SMP_CPU_COUNT(); cpu++ )
    {
        cyg_hal_smp_cpu_sync[cpu] = 0;
        cyg_hal_smp_cpu_sync_flag[cpu] = 0;
        cyg_hal_smp_cpu_running[cpu] = 0;
        cyg_hal_smp_cpu_entry[cpu] = 0;
        
        if( cpu != HAL_SMP_CPU_THIS() )
            cyg_hal_cpu_start( cpu );
        else cyg_hal_smp_cpu_running[cpu] = 1;
    }
}

/*------------------------------------------------------------------------*/
// SMP message buffers.
// SMP CPUs pass messages to eachother via a small circular buffer
// protected by a spinlock. Each message is a single 32 bit word with
// a type code in the top 4 bits and any argument in the remaining
// 28 bits.

#define SMP_MSGBUF_SIZE 4

static struct smp_msg_t
{
    HAL_SPINLOCK_TYPE           lock;           // protecting spinlock
    volatile CYG_WORD32         msgs[SMP_MSGBUF_SIZE]; // message buffer
    volatile CYG_WORD32         head;           // head of list
    volatile CYG_WORD32         tail;           // tail of list
    volatile CYG_WORD32         reschedule;     // reschedule request
    volatile CYG_WORD32         timeslice;      // timeslice request
} smp_msg[HAL_SMP_CPU_MAX];

/*------------------------------------------------------------------------*/
// Pass a message to another CPU.

#if SCREEN_DIAGNOSTICS
static int res_msgs[2], tms_msgs[2];
#endif

__externC void cyg_hal_cpu_message( HAL_SMP_CPU_TYPE cpu,
                                    CYG_WORD32 msg,
                                    CYG_WORD32 arg,
                                    CYG_WORD32 wait)
{
#if 1
    CYG_INTERRUPT_STATE istate;    
    struct smp_msg_t *m = &smp_msg[cpu];
    int i;
    HAL_SMP_CPU_TYPE me = HAL_SMP_CPU_THIS();
 
    HAL_DISABLE_INTERRUPTS( istate );
    
    // Get access to the message buffer for the selected CPU
    HAL_SPINLOCK_SPIN( m->lock );

#if 0 //SCREEN_DIAGNOSTICS    
    if( msg == HAL_SMP_MESSAGE_RESCHEDULE )
        res_msgs[me]++;
    else if( msg == HAL_SMP_MESSAGE_TIMESLICE )
        tms_msgs[me]++;
    PC_WRITE_SCREEN_8( PC_SCREEN_LINE(18+me), me);     
    PC_WRITE_SCREEN_16( PC_SCREEN_LINE(18+me)+40, res_msgs[me]); 
    PC_WRITE_SCREEN_16( PC_SCREEN_LINE(18+me)+45, tms_msgs[me]); 
#endif
 
    if( msg == HAL_SMP_MESSAGE_RESCHEDULE )
        m->reschedule = true;
    else if( msg == HAL_SMP_MESSAGE_TIMESLICE )
        m->timeslice = true;
    else
    {
        CYG_WORD32 next = (m->tail + 1) & (SMP_MSGBUF_SIZE-1);

        // If the buffer is full, wait for space to appear in it.
        // This should only need to be done very rarely.
    
        while( next == m->head )
        {
            HAL_SPINLOCK_CLEAR( m->lock );
            for( i = 0; i < 1000; i++ );
            HAL_SPINLOCK_SPIN( m->lock );        
        }

        m->msgs[m->tail] = msg | arg;

        m->tail = next;
    }
    
    // Now send an interrupt to the CPU.
    
//    PC_WRITE_SCREEN_16( PC_SCREEN_LINE(18+me)+50, cyg_hal_smp_cpu_running[cpu] );
    
    if( cyg_hal_smp_cpu_running[cpu] )
    {
        CYG_WORD32 icrlo, icrhi;

        // Set the ICR fields we want to write. Most fields are zero
        // except the destination in the high word and the vector
        // number in the low.
        icrhi = cpu<<24;
        icrlo = CYGNUM_HAL_SMP_CPU_INTERRUPT_VECTOR( cpu );

        // Write the ICR register. The interrupt will be raised when
        // the low word is written.
        HAL_APIC_WRITE( HAL_APIC_ICR_HI, icrhi );
        HAL_APIC_WRITE( HAL_APIC_ICR_LO, icrlo );

        // Wait for the ICR to become inactive
        do {
#if 0 //SCREEN_DIAGNOSTICS            
            static int n;                
            PC_WRITE_SCREEN_8( PC_SCREEN_LINE(18+me)+55, n );
            n++;
#endif            
            HAL_APIC_READ( HAL_APIC_ICR_LO, icrlo );
        } while( (icrlo & 0x00001000) != 0 );        
    }

    HAL_SPINLOCK_CLEAR( m->lock );

    // If we are expected to wait for the command to complete, then
    // spin here until it does. We actually wait for the destination
    // CPU to empty its input buffer. So we might wait for messages
    // from other CPUs as well. But this is benign.
    
    while(wait)
    {
        for( i = 0; i < 1000; i++ );
        
        HAL_SPINLOCK_SPIN( m->lock );

        if( m->head == m->tail )
            wait = false;
        
        HAL_SPINLOCK_CLEAR( m->lock );

    } 

    HAL_RESTORE_INTERRUPTS( istate );
#endif    
}

/*------------------------------------------------------------------------*/

#if SCREEN_DIAGNOSTICS
static int isrs[2];
static int dsrs[2];
#endif

__externC CYG_WORD32 cyg_hal_cpu_message_isr( CYG_WORD32 vector, CYG_ADDRWORD data )
{
    HAL_SMP_CPU_TYPE me = HAL_SMP_CPU_THIS();
    struct smp_msg_t *m = &smp_msg[me];
    CYG_WORD32 ret = 1;
    CYG_INTERRUPT_STATE istate;
    
    HAL_DISABLE_INTERRUPTS( istate );

    HAL_SPINLOCK_SPIN( m->lock );

    // First, acknowledge the interrupt.
    
    HAL_INTERRUPT_ACKNOWLEDGE( vector );

#if SCREEN_DIAGNOSTICS
    isrs[me]++;    
    PC_WRITE_SCREEN_8( PC_SCREEN_LINE(18+me), me); 
    PC_WRITE_SCREEN_16( PC_SCREEN_LINE(18+me)+5, isrs[me]); 
#endif
    
    if( m->reschedule || m->timeslice )
        ret |= 2;               // Ask for the DSR to be called.
    
    // Now pick messages out of the buffer and handle them
    
    while( m->head != m->tail )
    {
        CYG_WORD32 msg = m->msgs[m->head];

        switch( msg & HAL_SMP_MESSAGE_TYPE )
        {
        case HAL_SMP_MESSAGE_RESCHEDULE:
            ret |= 2;           // Ask for the DSR to be called.
            break;
        case HAL_SMP_MESSAGE_MASK:
            // Mask the supplied vector
//            cyg_hal_interrupt_set_mask( msg&HAL_SMP_MESSAGE_ARG, false );
            break;
        case HAL_SMP_MESSAGE_UNMASK:
            // Unmask the supplied vector
//            cyg_hal_interrupt_set_mask( msg&HAL_SMP_MESSAGE_ARG, true );
            break;
        case HAL_SMP_MESSAGE_REVECTOR:
            // Deal with a change of CPU assignment for a vector. We
            // only actually worry about what happens when the vector
            // is changed to some other CPU. We just mask the
            // interrupt locally.
//            if( hal_interrupt_cpu[msg&HAL_SMP_MESSAGE_ARG] != me )
//                cyg_hal_interrupt_set_mask( msg&HAL_SMP_MESSAGE_ARG, false );
            break;
        }

        // Update the head pointer after handling the message, so that
        // the wait in cyg_hal_cpu_message() completes after the action
        // requested.
        
        m->head = (m->head + 1) & (SMP_MSGBUF_SIZE-1);
    }

    HAL_SPINLOCK_CLEAR( m->lock );    

    HAL_RESTORE_INTERRUPTS( istate );

    return ret;
}

/*------------------------------------------------------------------------*/
// CPU message DSR.
// This is only executed if the message was
// HAL_SMP_MESSAGE_RESCHEDULE. It calls up into the kernel to effect a
// reschedule.

__externC void cyg_scheduler_set_need_reschedule(void);
__externC void cyg_scheduler_timeslice_cpu(void);

#if SCREEN_DIAGNOSTICS
__externC int cyg_scheduler_sched_lock;
static int rescheds[2];
static int timeslices[2];
#endif

__externC CYG_WORD32 cyg_hal_cpu_message_dsr( CYG_WORD32 vector, CYG_ADDRWORD data )
{
    HAL_SMP_CPU_TYPE me = HAL_SMP_CPU_THIS();
    struct smp_msg_t *m = &smp_msg[me];
    CYG_INTERRUPT_STATE istate;
    CYG_WORD32 reschedule, timeslice;
    
    HAL_DISABLE_INTERRUPTS( istate );
    HAL_SPINLOCK_SPIN( m->lock );

#if SCREEN_DIAGNOSTICS    
    dsrs[me]++;    
    PC_WRITE_SCREEN_16( PC_SCREEN_LINE(18+me)+10, dsrs[me]);
    PC_WRITE_SCREEN_16( PC_SCREEN_LINE(18+me)+15, cyg_scheduler_sched_lock);  
#endif
    
    reschedule = m->reschedule;
    timeslice = m->timeslice;
    m->reschedule = m->timeslice = false;

    HAL_SPINLOCK_CLEAR( m->lock );    
    HAL_RESTORE_INTERRUPTS( istate );
        
    if( reschedule )
    {
#if SCREEN_DIAGNOSTICS        
        rescheds[me]++;
        PC_WRITE_SCREEN_16( PC_SCREEN_LINE(18+me)+20, rescheds[me]);
#endif        
        cyg_scheduler_set_need_reschedule();
    }
    if( timeslice )
    {
#if SCREEN_DIAGNOSTICS
        timeslices[me]++;
        PC_WRITE_SCREEN_16( PC_SCREEN_LINE(18+me)+25, timeslices[me]);
#endif        
        cyg_scheduler_timeslice_cpu();
    }

    return 0;
    
}

/*------------------------------------------------------------------------*/

#if SCREEN_DIAGNOSTICS
static int x = 0;
#endif

__externC void cyg_hal_smp_halt_other_cpus(void)
{
    int i;
    HAL_SMP_CPU_TYPE me = HAL_SMP_CPU_THIS();
    
//    PC_WRITE_SCREEN_8( PC_SCREEN_LINE(6+me), me );
  
    for( i = 0 ; i < HAL_SMP_CPU_COUNT(); i++ )
    {
        if( i != me && cyg_hal_smp_cpu_running[i] )
        {
            CYG_WORD32 icrhi, icrlo;
            CYG_WORD32 oldsync;
            
//            PC_WRITE_SCREEN_8( PC_SCREEN_LINE(6+me)+40, i );

            oldsync = cyg_hal_smp_cpu_sync_flag[i]; 
            cyg_hal_smp_cpu_sync[i] = 0;


            icrhi = i<<24;
            icrlo = CYGNUM_HAL_VECTOR_NMI;  // not really used
            icrlo |= 0x00000400;    // Delivery = NMI
            //icrlo |= 0x000C0000;    // Dest = all excluding self

            // Write the ICR register. The interrupt will be raised when
            // the low word is written.
            HAL_APIC_WRITE( HAL_APIC_ICR_HI, icrhi );
            HAL_APIC_WRITE( HAL_APIC_ICR_LO, icrlo );

            // Wait for the ICR to become inactive
            do {
#if 0 //SCREEN_DIAGNOSTICS
                static int n;                
                PC_WRITE_SCREEN_8( PC_SCREEN_LINE(6+me)+45, n );
                n++;
#endif                
                HAL_APIC_READ( HAL_APIC_ICR_LO, icrlo );
            } while( (icrlo & 0x00001000) != 0 );

            // Wait for CPU to halt
            while( cyg_hal_smp_cpu_sync_flag[i] == oldsync )
            {
#if 0 //SCREEN_DIAGNOSTICS                
                PC_WRITE_SCREEN_8( PC_SCREEN_LINE(6+me)+4, x ); x++;
                PC_WRITE_SCREEN_8( PC_SCREEN_LINE(6+me)+10+(i*8), cyg_hal_smp_cpu_sync_flag[i] );
                PC_WRITE_SCREEN_8( PC_SCREEN_LINE(6+me)+10+(i*8)+4, oldsync );
#endif           
                hal_delay_us( 100 );
            }
            
        }
    }
    
}

__externC void cyg_hal_smp_release_other_cpus(void)
{
    int i;
    for( i = 0 ; i < HAL_SMP_CPU_COUNT(); i++ )
    {
        if( i != HAL_SMP_CPU_THIS() && cyg_hal_smp_cpu_running[i] )
        {
            CYG_WORD32 oldsync = cyg_hal_smp_cpu_sync_flag[i];        
            cyg_hal_smp_cpu_sync[i] = 1;
            while( cyg_hal_smp_cpu_sync_flag[i] == oldsync )
                continue;
            cyg_hal_smp_cpu_sync[i] = 0;
        }
    }
}

#endif // CYGPKG_HAL_SMP_SUPPORT

/*------------------------------------------------------------------------*/
/* End of pcmb_smp.c                                                      */