packages/hal/i386/pcmb/v2_0/src/pcmb_smp.c

   1 //==========================================================================
   2 //
   3 //      pcmb_smp.c
   4 //
   5 //      HAL SMP implementation
   6 //
   7 //==========================================================================
   8 //####ECOSGPLCOPYRIGHTBEGIN####
   9 // -------------------------------------------
  10 // This file is part of eCos, the Embedded Configurable Operating System.
  11 // Copyright (C) 1998, 1999, 2000, 2001, 2002 Red Hat, Inc.
  12 //
  13 // eCos is free software; you can redistribute it and/or modify it under
  14 // the terms of the GNU General Public License as published by the Free
  15 // Software Foundation; either version 2 or (at your option) any later version.
  16 //
  17 // eCos is distributed in the hope that it will be useful, but WITHOUT ANY
  18 // WARRANTY; without even the implied warranty of MERCHANTABILITY or
  19 // FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
  20 // for more details.
  21 //
  22 // You should have received a copy of the GNU General Public License along
  23 // with eCos; if not, write to the Free Software Foundation, Inc.,
  24 // 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  25 //
  26 // As a special exception, if other files instantiate templates or use macros
  27 // or inline functions from this file, or you compile this file and link it
  28 // with other works to produce a work based on this file, this file does not
  29 // by itself cause the resulting work to be covered by the GNU General Public
  30 // License. However the source code for this file must still be made available
  31 // in accordance with section (3) of the GNU General Public License.
  32 //
  33 // This exception does not invalidate any other reasons why a work based on
  34 // this file might be covered by the GNU General Public License.
  35 //
  36 // Alternative licenses for eCos may be arranged by contacting Red Hat, Inc.
  37 // at http://sources.redhat.com/ecos/ecos-license/
  38 // -------------------------------------------
  39 //####ECOSGPLCOPYRIGHTEND####
  40 //==========================================================================
  41 //#####DESCRIPTIONBEGIN####
  42 //
  43 // Author(s):    nickg
  44 // Contributors: nickg
  45 // Date:         2001-08-03
  46 // Purpose:      HAL SMP implementation
  47 // Description:  This file contains SMP support functions.
  48 //
  49 //####DESCRIPTIONEND####
  50 //
  51 //========================================================================*/
  52
  53 #include <pkgconf/hal.h>
  54 #include <pkgconf/hal_i386.h>
  55 #include <pkgconf/hal_i386_pcmb.h>
  56
  57 #ifdef CYGPKG_HAL_SMP_SUPPORT
  58
  59 #ifdef CYGPKG_KERNEL
  60 #include <pkgconf/kernel.h>
  61 #endif
  62
  63 #include <cyg/infra/cyg_type.h>         // Base types
  64 #include <cyg/infra/cyg_trac.h>         // tracing macros
  65 #include <cyg/infra/cyg_ass.h>          // assertion macros
  66 #include <cyg/infra/diag.h>
  67
  68 #include <cyg/hal/hal_intr.h>
  69 #include <cyg/hal/hal_io.h>
  70 #include <cyg/hal/hal_smp.h>
  71
  72 // ------------------------------------------------------------------------
  73 // Debugging/diagnostic controls
  74
  75 // Setting this to 1 causes the parsing of the MP structures and the
  76 // initialization of the APIC and IOAPIC to be reported on the
  77 // diagnostic channel.
  78 #define SHOW_DIAGNOSTICS 0
  79
  80 // Setting this to 1 causes various things to be displayed on the PC
  81 // screen during normal operation. These are mostly for my own use,
  82 // and may be somewhat obscure to anyone else.
  83 #define SCREEN_DIAGNOSTICS 0
  84
  85 #if SCREEN_DIAGNOSTICS==0
  86 #undef PC_WRITE_SCREEN
  87 #undef PC_WRITE_SCREEN_8
  88 #undef PC_WRITE_SCREEN_16
  89 #undef PC_WRITE_SCREEN_32
  90 #define PC_WRITE_SCREEN( pos, ch )
  91 #define PC_WRITE_SCREEN_8( pos, val )
  92 #define PC_WRITE_SCREEN_16( pos, val )
  93 #define PC_WRITE_SCREEN_32( pos, val )
  94 #endif
  95
  96 // ------------------------------------------------------------------------
  97
  98 static int streq( const char *s1, const char *s2 )
  99 {
 100     while( *s1 == *s2 && *s1 && *s2 ) s1++,s2++;
 101
 102     return !(*s2-*s1);
 103 }
 104
 105 /*------------------------------------------------------------------------*/
 106 // MP FPS structure:
 107
 108 #define MP_FPS_SIGNATURE         0
 109 #define MP_FPS_MPCT              4
 110 #define MP_FPS_LENGTH            8
 111 #define MP_FPS_REV               9
 112 #define MP_FPS_CSUM             10
 113 #define MP_FPS_FEATURE1         11
 114 #define MP_FPS_FEATURE2         12
 115 #define MP_FPS_FEATURE3         13
 116 #define MP_FPS_FEATURE4         14
 117 #define MP_FPS_FEATURE5         15
 118 #define MP_FPS_SIZE             16
 119
 120 #define MP_FPS_SIGNATURE_VALUE  0x5f504d5f
 121
 122 /*------------------------------------------------------------------------*/
 123 // MP config table structure
 124
 125 // Header structure:
 126
 127 #define MPCT_HDR_SIGNATURE      0
 128 #define MPCT_HDR_LENGTH         4
 129 #define MPCT_HDR_REV            6
 130 #define MPCT_HDR_CHECKSUM       7
 131 #define MPCT_HDR_OEM_ID         8
 132 #define MPCT_HDR_PROD_ID        16
 133 #define MPCT_HDR_OEM_TAB        28
 134 #define MPCT_HDR_OEM_TAB_SIZE   32
 135 #define MPCT_HDR_ENTRY_COUNT    34
 136 #define MPCT_HDR_LOCAL_APIC     36
 137 #define MPCT_HDR_XTAB_LENGTH    40
 138 #define MPCT_HDR_XTAB_CHECKSUM  42
 139 #define MPCT_HDR_SIZE           44
 140
 141 #define MPCT_HDR_SIGNATURE_VAL  0x504d4350
 142
 143 #define MPCT_ENTRY_TYPE_PROCESSOR       0
 144 #define MPCT_ENTRY_TYPE_BUS             1
 145 #define MPCT_ENTRY_TYPE_IOAPIC          2
 146 #define MPCT_ENTRY_TYPE_INTERRUPT_IO    3
 147 #define MPCT_ENTRY_TYPE_INTERRUPT_LOCAL 4
 148
 149 #define MPCT_ENTRY_PROC_TYPE            0
 150 #define MPCT_ENTRY_PROC_APIC_ID         1
 151 #define MPCT_ENTRY_PROC_APIC_VER        2
 152 #define MPCT_ENTRY_PROC_CPU_FLAGS       3
 153 #define MPCT_ENTRY_PROC_CPU_SIGNATURE   4
 154 #define MPCT_ENTRY_PROC_FEATURE_FLAGS   8
 155 #define MPCT_ENTRY_PROC_RESERVED0       12
 156 #define MPCT_ENTRY_PROC_RESERVED1       16
 157 #define MPCT_ENTRY_PROC_SIZE            20
 158
 159 #define MPCT_ENTRY_BUS_TYPE             0
 160 #define MPCT_ENTRY_BUS_ID               1
 161 #define MPCT_ENTRY_BUS_TYPE_STRING      2
 162 #define MPCT_ENTRY_BUS_SIZE             8
 163
 164 #define MPCT_ENTRY_IOAPIC_TYPE          0
 165 #define MPCT_ENTRY_IOAPIC_ID            1
 166 #define MPCT_ENTRY_IOAPIC_VER           2
 167 #define MPCT_ENTRY_IOAPIC_FLAGS         3
 168 #define MPCT_ENTRY_IOAPIC_ADDRESS       4
 169 #define MPCT_ENTRY_IOAPIC_SIZE          8
 170
 171 #define MPCT_ENTRY_IOINT_TYPE           0
 172 #define MPCT_ENTRY_IOINT_INT_TYPE       1
 173 #define MPCT_ENTRY_IOINT_FLAGS          2
 174 #define MPCT_ENTRY_IOINT_SOURCE_BUS     4
 175 #define MPCT_ENTRY_IOINT_SOURCE_IRQ     5
 176 #define MPCT_ENTRY_IOINT_DEST_APIC      6
 177 #define MPCT_ENTRY_IOINT_DEST_INT       7
 178 #define MPCT_ENTRY_IOINT_SIZE           8
 179
 180 #define MPCT_ENTRY_LOCINT_TYPE          0
 181 #define MPCT_ENTRY_LOCINT_INT_TYPE      1
 182 #define MPCT_ENTRY_LOCINT_FLAGS         2
 183 #define MPCT_ENTRY_LOCINT_SOURCE_BUS    4
 184 #define MPCT_ENTRY_LOCINT_SOURCE_IRQ    5
 185 #define MPCT_ENTRY_LOCINT_DEST_APIC     6
 186 #define MPCT_ENTRY_LOCINT_DEST_INT      7
 187 #define MPCT_ENTRY_LOCINT_SIZE          8
 188
 189 /*------------------------------------------------------------------------*/
 190 // Exported SMP configuration
 191
 192 CYG_ADDRESS cyg_hal_smp_local_apic;
 193
 194 CYG_ADDRESS cyg_hal_smp_io_apic;
 195
 196 CYG_WORD32 cyg_hal_smp_cpu_count = 0;
 197
 198 CYG_BYTE cyg_hal_smp_cpu_flags[HAL_SMP_CPU_MAX];
 199
 200 CYG_BYTE cyg_hal_isa_bus_id = 0xff;
 201 CYG_BYTE cyg_hal_isa_bus_irq[16];
 202
 203 CYG_BYTE cyg_hal_pci_bus_id = 0xff;
 204 CYG_BYTE cyg_hal_pci_bus_irq[4];
 205
 206 HAL_SPINLOCK_TYPE cyg_hal_ioapic_lock;
 207
 208 /*------------------------------------------------------------------------*/
 209
 210 // Statics
 211
 212 static CYG_ADDRESS     mp_fps;
 213 static CYG_ADDRESS     mpct;
 214
 215
 216 /*------------------------------------------------------------------------*/
 217
 218 static CYG_WORD32 mp_checksum(CYG_BYTE *p, CYG_WORD32 len)
 219 {
 220         CYG_WORD32 sum = 0;
 221
 222         while (len--) sum += *p++;
 223
 224         return sum & 0xFF;
 225 }
 226
 227 /*------------------------------------------------------------------------*/
 228
 229 static cyg_bool cyg_hal_scan_smp_config( CYG_ADDRESS base, CYG_ADDRWORD size )
 230 {
 231
 232 #if SHOW_DIAGNOSTICS
 233     diag_printf("Scan for MP struct: %08x..%08x\n",base,base+size);
 234 #endif
 235
 236     while( size > 0 )
 237     {
 238         CYG_WORD32 sig;
 239
 240         HAL_READMEM_UINT32( base, sig );
 241
 242         if( sig == MP_FPS_SIGNATURE_VALUE )
 243         {
 244             CYG_BYTE val8;
 245             // We have a candidate for the floating structure
 246
 247 #if SHOW_DIAGNOSTICS
 248             diag_printf("MP_FPS candidate found at %08x\n",base);
 249 #endif
 250
 251             HAL_READMEM_UINT8( base+MP_FPS_LENGTH, val8 );
 252
 253             if( val8 != 1 )
 254                 break;
 255
 256             HAL_READMEM_UINT8( base+MP_FPS_REV, val8 );
 257
 258             if( val8 != 1 && val8 != 4 )
 259                 break;
 260
 261             if( mp_checksum( (CYG_BYTE *)base, MP_FPS_SIZE ) != 0 )
 262                 break;
 263
 264             mp_fps = base;
 265
 266             HAL_READMEM_UINT32( base+MP_FPS_MPCT, mpct );
 267
 268 #if SHOW_DIAGNOSTICS
 269             diag_printf("MPCT at %08x\n",mpct);
 270 #endif
 271             return 1;
 272         }
 273
 274         base += 16;
 275         size -= 16;
 276     }
 277
 278     return 0;
 279 }
 280
 281 /*------------------------------------------------------------------------*/
 282
 283 static cyg_bool cyg_hal_find_smp_config( void )
 284 {
 285     // check bottom 1k
 286     if( cyg_hal_scan_smp_config(0x00000, 0x00400 ) )
 287         return 1;
 288
 289     // check top 1k of RAM
 290     if( cyg_hal_scan_smp_config(0x9fc00, 0x00400 ) )
 291         return 1;
 292
 293     // check BIOS ROM
 294     if( cyg_hal_scan_smp_config(0xf0000, 0x10000 ) )
 295         return 1;
 296 }
 297
 298 /*------------------------------------------------------------------------*/
 299
 300 static cyg_bool cyg_hal_parse_smp_config( void )
 301 {
 302     CYG_WORD32 val32;
 303     CYG_WORD16 val16;
 304     CYG_BYTE val8;
 305     int i;
 306
 307 #if SHOW_DIAGNOSTICS
 308
 309     {
 310
 311         diag_printf("FPS address:         %08x\n",mp_fps);
 312         HAL_READMEM_UINT32( mp_fps+MP_FPS_SIGNATURE, val32);
 313         diag_printf("FPS signature:       %08x\n",val32);
 314         HAL_READMEM_UINT32( mp_fps+MP_FPS_MPCT, val32);
 315         diag_printf("FPS MPCT address:    %08x\n",val32);
 316         HAL_READMEM_UINT8( mp_fps+MP_FPS_LENGTH, val8);
 317         diag_printf("FPS length:          %02x\n",val8);
 318         HAL_READMEM_UINT8( mp_fps+MP_FPS_REV, val8);
 319         diag_printf("FPS spec rev:        %02x\n",val8);
 320         HAL_READMEM_UINT8( mp_fps+MP_FPS_CSUM, val8);
 321         diag_printf("FPS checksum:        %02x\n",val8);
 322
 323         for( i = 0; i < 5; i++ )
 324         {
 325             HAL_READMEM_UINT8( mp_fps+MP_FPS_FEATURE1, val8);
 326             diag_printf("FPS feature byte %d:  %02x\n",i,val8);
 327         }
 328     }
 329
 330 #endif
 331
 332     if( mpct )
 333     {
 334
 335         HAL_READMEM_UINT32( mpct+MPCT_HDR_SIGNATURE, val32);
 336
 337         if( val32 != MPCT_HDR_SIGNATURE_VAL )
 338             return 0;
 339
 340         HAL_READMEM_UINT16( mpct+MPCT_HDR_LENGTH, val16);
 341         if( mp_checksum( (CYG_BYTE *)mpct, val16 ) != 0 )
 342             return 0;
 343
 344 #if SHOW_DIAGNOSTICS
 345         {
 346             char id[13];
 347
 348             diag_printf("MPCT address:                 %08x\n",mpct);
 349             HAL_READMEM_UINT32( mpct+MPCT_HDR_SIGNATURE, val32);
 350             diag_printf("MPCT signature:               %08x\n",val32);
 351
 352             HAL_READMEM_UINT16( mpct+MPCT_HDR_LENGTH, val16);
 353             diag_printf("MPCT length:                  %04x\n",val16);
 354             HAL_READMEM_UINT8( mpct+MPCT_HDR_REV, val8);
 355             diag_printf("MPCT spec rev:                %02x\n",val8);
 356             HAL_READMEM_UINT8( mpct+MPCT_HDR_CHECKSUM, val8);
 357             diag_printf("MPCT checksum:                %02x\n",val8);
 358
 359             for( i = 0; i < 8; i++ )
 360             {
 361                 HAL_READMEM_UINT8( mpct+MPCT_HDR_OEM_ID+i, val8);
 362                 id[i] = val8;
 363             }
 364             id[i] = 0;
 365             diag_printf("MPCT OEM Id:                  %s\n",id);
 366
 367             for( i = 0; i < 12; i++ )
 368             {
 369                 HAL_READMEM_UINT8( mpct+MPCT_HDR_PROD_ID+i, val8);
 370                 id[i] = val8;
 371             }
 372             id[i] = 0;
 373             diag_printf("MPCT Product Id:              %s\n",id);
 374
 375             HAL_READMEM_UINT32( mpct+MPCT_HDR_OEM_TAB, val32);
 376             diag_printf("MPCT OEM table:               %08x\n",val32);
 377             HAL_READMEM_UINT16( mpct+MPCT_HDR_OEM_TAB_SIZE, val16);
 378             diag_printf("MPCT OEM table size:          %04x\n",val16);
 379
 380             HAL_READMEM_UINT16( mpct+MPCT_HDR_ENTRY_COUNT, val16);
 381             diag_printf("MPCT entry count:             %04x\n",val16);
 382
 383             HAL_READMEM_UINT32( mpct+MPCT_HDR_LOCAL_APIC, val32);
 384             diag_printf("MPCT local APIC:              %08x\n",val32);
 385
 386             HAL_READMEM_UINT16( mpct+MPCT_HDR_XTAB_LENGTH, val16);
 387             diag_printf("MPCT extended table length:   %04x\n",val16);
 388             HAL_READMEM_UINT8( mpct+MPCT_HDR_XTAB_CHECKSUM, val8);
 389             diag_printf("MPCT extended table checksum: %02x\n",val8);
 390         }
 391 #endif
 392
 393         // Extract the data we need from the structure
 394
 395         HAL_READMEM_UINT32( mpct+MPCT_HDR_LOCAL_APIC, cyg_hal_smp_local_apic);
 396
 397         // now parse the base table, looking for processor and IOAPIC entries.
 398
 399         {
 400             CYG_WORD16 entries;
 401             CYG_ADDRESS entry = mpct + MPCT_HDR_SIZE;
 402
 403             HAL_READMEM_UINT16( mpct+MPCT_HDR_ENTRY_COUNT, entries);
 404
 405 #if SHOW_DIAGNOSTICS
 406             diag_printf("\nBase table:\n");
 407 #endif
 408
 409             while( entries-- )
 410             {
 411                 CYG_BYTE type;
 412
 413                 HAL_READMEM_UINT8( entry, type );
 414
 415                 switch( type )
 416                 {
 417                 case MPCT_ENTRY_TYPE_PROCESSOR:
 418 #if SHOW_DIAGNOSTICS
 419                     diag_printf("        Processor\n");
 420                     HAL_READMEM_UINT8( entry+MPCT_ENTRY_PROC_APIC_ID, val8 );
 421                     diag_printf("                APIC ID:                  %02x\n",val8);
 422                     HAL_READMEM_UINT8( entry+MPCT_ENTRY_PROC_APIC_VER, val8 );
 423                     diag_printf("                APIC Version:             %02x\n",val8);
 424                     HAL_READMEM_UINT8( entry+MPCT_ENTRY_PROC_CPU_FLAGS, val8 );
 425                     diag_printf("                CPU Flags:                %02x\n",val8);
 426                     HAL_READMEM_UINT8( entry+MPCT_ENTRY_PROC_CPU_SIGNATURE, val32 );
 427                     diag_printf("                CPU Signature:            %08x\n",val32);
 428                     HAL_READMEM_UINT8( entry+MPCT_ENTRY_PROC_FEATURE_FLAGS, val32 );
 429                     diag_printf("                Feature flags:            %08x\n",val32);
 430 #endif
 431                     {
 432                         CYG_BYTE cpuid;
 433
 434                         // Index CPUs by their APIC IDs
 435                         HAL_READMEM_UINT8( entry+MPCT_ENTRY_PROC_APIC_ID, cpuid );
 436
 437                         // Get flags for this CPU.
 438                         HAL_READMEM_UINT8(entry+MPCT_ENTRY_PROC_CPU_FLAGS, cyg_hal_smp_cpu_flags[cpuid]);
 439
 440                         cyg_hal_smp_cpu_count++;      // count another CPU
 441                     }
 442
 443                     entry += MPCT_ENTRY_PROC_SIZE;
 444                     break;
 445
 446                 case MPCT_ENTRY_TYPE_BUS:
 447                     {
 448                         CYG_BYTE id;
 449                         char bustype[7];
 450                         HAL_READMEM_UINT8( entry+MPCT_ENTRY_BUS_ID, id );
 451 #if SHOW_DIAGNOSTICS
 452                         diag_printf("        Bus\n");
 453                         diag_printf("                ID:                       %02x\n",id);
 454 #endif
 455                         {
 456                             int i;
 457                             for( i = 0; i < 6; i++ )
 458                             {
 459                                 HAL_READMEM_UINT8( entry+MPCT_ENTRY_BUS_TYPE_STRING+i, val8 );
 460                                 bustype[i] = val8;
 461                             }
 462                             bustype[i] = 0;
 463 #if SHOW_DIAGNOSTICS
 464                             diag_printf("                Type:                     %s\n",&bustype[0]);
 465 #endif
 466                         }
 467
 468                         if( streq( bustype, "ISA   " ) )
 469                         {
 470                             cyg_hal_isa_bus_id = id;
 471                             for( i = 0; i < 16; i++ )
 472                                 cyg_hal_isa_bus_irq[i] = 100;
 473                         }
 474                         if( streq( bustype, "PCI   " ) )
 475                         {
 476                             cyg_hal_pci_bus_id = id;
 477                         }
 478                     }
 479
 480                     entry += MPCT_ENTRY_BUS_SIZE;
 481                     break;
 482
 483                 case MPCT_ENTRY_TYPE_IOAPIC:
 484 #if SHOW_DIAGNOSTICS
 485                     diag_printf("        I/O APIC\n");
 486                     HAL_READMEM_UINT8( entry+MPCT_ENTRY_IOAPIC_ID, val8 );
 487                     diag_printf("                ID:                       %02x\n",val8);
 488                     HAL_READMEM_UINT8( entry+MPCT_ENTRY_IOAPIC_VER, val8 );
 489                     diag_printf("                Version:                  %02x\n",val8);
 490                     HAL_READMEM_UINT8( entry+MPCT_ENTRY_IOAPIC_FLAGS, val8 );
 491                     diag_printf("                Flags:                    %02x\n",val8);
 492                     HAL_READMEM_UINT32( entry+MPCT_ENTRY_IOAPIC_ADDRESS, val32 );
 493                     diag_printf("                Address:                  %08x\n",val32);
 494 #endif
 495
 496                     HAL_READMEM_UINT32( entry+MPCT_ENTRY_IOAPIC_ADDRESS, cyg_hal_smp_io_apic );
 497                     entry += MPCT_ENTRY_IOAPIC_SIZE;
 498                     break;
 499
 500                 case MPCT_ENTRY_TYPE_INTERRUPT_IO:
 501                     {
 502                         CYG_BYTE bus, irq, dst;
 503                         HAL_READMEM_UINT8( entry+MPCT_ENTRY_IOINT_SOURCE_BUS, bus );
 504                         HAL_READMEM_UINT8( entry+MPCT_ENTRY_IOINT_SOURCE_IRQ, irq );
 505                         HAL_READMEM_UINT8( entry+MPCT_ENTRY_IOINT_DEST_INT, dst );
 506 #if SHOW_DIAGNOSTICS
 507                         diag_printf("        I/O interrupt assignment\n");
 508                         HAL_READMEM_UINT8( entry+MPCT_ENTRY_IOINT_TYPE, val8 );
 509                         diag_printf("                Type:                     %02x\n",val8);
 510                         HAL_READMEM_UINT16( entry+MPCT_ENTRY_IOINT_TYPE, val16 );
 511                         diag_printf("                Flags:                    %04x\n",val16);
 512                         diag_printf("                Source bus:               %02x\n",bus);
 513                         diag_printf("                Source IRQ:               %02x\n",irq);
 514                         HAL_READMEM_UINT8( entry+MPCT_ENTRY_IOINT_DEST_APIC, val8 );
 515                         diag_printf("                Dest APIC:                %02x\n",val8);
 516                         diag_printf("                Dest Interrupt:           %02x\n",dst);
 517 #endif
 518
 519                         if( bus == cyg_hal_isa_bus_id )
 520                             cyg_hal_isa_bus_irq[irq] = dst;
 521 //                        if( bus == cyg_hal_pci_bus_id )
 522 //                            cyg_hal_pci_bus_irq[irq] = dst;
 523
 524                     }
 525                     entry += MPCT_ENTRY_IOINT_SIZE;
 526                     break;
 527
 528                 case MPCT_ENTRY_TYPE_INTERRUPT_LOCAL:
 529 #if SHOW_DIAGNOSTICS
 530                     diag_printf("        Local interrupt assignment\n");
 531                     HAL_READMEM_UINT8( entry+MPCT_ENTRY_IOINT_TYPE, val8 );
 532                     diag_printf("                Type:                     %02x\n",val8);
 533                     HAL_READMEM_UINT16( entry+MPCT_ENTRY_IOINT_TYPE, val16 );
 534                     diag_printf("                Flags:                    %04x\n",val16);
 535                     HAL_READMEM_UINT8( entry+MPCT_ENTRY_IOINT_SOURCE_BUS, val8 );
 536                     diag_printf("                Source bus:               %02x\n",val8);
 537                     HAL_READMEM_UINT8( entry+MPCT_ENTRY_IOINT_SOURCE_IRQ, val8 );
 538                     diag_printf("                Source IRQ:               %02x\n",val8);
 539                     HAL_READMEM_UINT8( entry+MPCT_ENTRY_IOINT_DEST_APIC, val8 );
 540                     diag_printf("                Dest APIC:                %02x\n",val8);
 541                     HAL_READMEM_UINT8( entry+MPCT_ENTRY_IOINT_DEST_INT, val8 );
 542                     diag_printf("                Dest Interrupt:           %02x\n",val8);
 543 #endif
 544                     entry += MPCT_ENTRY_LOCINT_SIZE;
 545                     break;
 546
 547                 default:
 548 #if SHOW_DIAGNOSTICS
 549                     diag_printf("        MPCT Entry: unknown type %02x\n",type);
 550 #endif
 551                     entry += 8;
 552                     break;
 553                 }
 554
 555             }
 556
 557         }
 558     }
 559
 560 #if SHOW_DIAGNOSTICS
 561
 562     diag_printf("Exported configuration:\n");
 563     diag_printf("        Local APIC: %08x\n", cyg_hal_smp_local_apic );
 564     diag_printf("        I/O APIC:   %08x\n", cyg_hal_smp_io_apic );
 565     diag_printf("        CPU count:  %d\n", cyg_hal_smp_cpu_count );
 566
 567     for( i = 0; i < cyg_hal_smp_cpu_count; i++ )
 568     {
 569         diag_printf("            CPU %d %sactive %s\n",i,
 570                     ((cyg_hal_smp_cpu_flags[i]&1)?"":"in"),
 571                     ((cyg_hal_smp_cpu_flags[i]&2)?"master":"slave")
 572             );
 573     }
 574
 575     diag_printf("        ISA IRQ map:\n");
 576
 577     for( i = 0; i < 16; i++ )
 578     {
 579         diag_printf("            IRQ %2d -> IOAPIC INT %2d\n",i,cyg_hal_isa_bus_irq[i]);
 580     }
 581
 582 #endif
 583
 584     return 1;
 585 }
 586
 587 /*------------------------------------------------------------------------*/
 588 // Init local APIC
 589
 590 static cyg_bool cyg_hal_smp_init_apic(void)
 591 {
 592
 593     cyg_uint32 maxlvt;
 594     cyg_uint32 val;
 595     HAL_SMP_CPU_TYPE cpu;
 596
 597     HAL_APIC_READ(HAL_APIC_ID, val );
 598     cpu = val>>24;
 599
 600 #if SHOW_DIAGNOSTICS
 601     diag_printf("Local APIC: %08x\n",cyg_hal_smp_local_apic);
 602     diag_printf("        ID:             %08x\n",val);
 603 #endif
 604     // get max local vector table entry offset
 605     HAL_APIC_READ(HAL_APIC_VER, maxlvt );
 606 #if SHOW_DIAGNOSTICS
 607     diag_printf("        VERSION:        %08x\n",maxlvt);
 608 #endif
 609     maxlvt >>= 16;
 610     maxlvt &= 0xFF;
 611
 612
 613 #if SHOW_DIAGNOSTICS
 614     diag_printf("maxlvt = %d\n",maxlvt);
 615 #endif
 616
 617     // Start by ensuring that all interrupt sources are disabled. The
 618     // following code ensures that this happens cleanly.
 619
 620     // Local timer vector
 621     HAL_APIC_READ( HAL_APIC_LVT_TIMER, val );
 622     val |= HAL_APIC_LVT_MASK ;
 623     HAL_APIC_WRITE( HAL_APIC_LVT_TIMER, val );
 624
 625 #if SHOW_DIAGNOSTICS
 626     diag_printf("        APIC_LVT_TIMER: %08x\n",val);
 627 #endif
 628
 629     // Local interrupt vectors
 630     HAL_APIC_READ( HAL_APIC_LVT_INT0, val );
 631     val |= HAL_APIC_LVT_MASK ;
 632     HAL_APIC_WRITE( HAL_APIC_LVT_INT0, val );
 633
 634 #if SHOW_DIAGNOSTICS
 635     diag_printf("        APIC_LVT_INT0:  %08x\n",val);
 636 #endif
 637
 638     HAL_APIC_READ( HAL_APIC_LVT_INT1, val );
 639     val |= HAL_APIC_LVT_MASK ;
 640     HAL_APIC_WRITE( HAL_APIC_LVT_INT1, val );
 641
 642 #if SHOW_DIAGNOSTICS
 643     diag_printf("        APIC_LVT_INT1:  %08x\n",val);
 644 #endif
 645
 646     if (maxlvt >= 3 )
 647     {
 648         HAL_APIC_READ( HAL_APIC_LVT_ERROR, val );
 649         val |= HAL_APIC_LVT_MASK ;
 650         HAL_APIC_WRITE( HAL_APIC_LVT_ERROR, val );
 651 #if SHOW_DIAGNOSTICS
 652         diag_printf("        APIC_LVT_ERROR: %08x\n",val);
 653 #endif
 654     }
 655     if (maxlvt >= 4 )
 656     {
 657         HAL_APIC_READ( HAL_APIC_LVT_PC, val );
 658         val |= HAL_APIC_LVT_MASK ;
 659         HAL_APIC_WRITE( HAL_APIC_LVT_PC, val );
 660 #if SHOW_DIAGNOSTICS
 661         diag_printf("        APIC_LVT_PC:    %08x\n",val);
 662 #endif
 663     }
 664
 665     // Now initialize the local vector table.
 666
 667     HAL_APIC_WRITE( HAL_APIC_LVT_TIMER, HAL_APIC_LVT_MASK );
 668     HAL_APIC_WRITE( HAL_APIC_LVT_INT0, HAL_APIC_LVT_MASK );
 669     HAL_APIC_WRITE( HAL_APIC_LVT_INT1, HAL_APIC_LVT_MASK );
 670     if( maxlvt >= 3 )
 671         HAL_APIC_WRITE( HAL_APIC_LVT_ERROR, HAL_APIC_LVT_MASK );
 672     if( maxlvt >= 4 )
 673         HAL_APIC_WRITE( HAL_APIC_LVT_PC, HAL_APIC_LVT_MASK );
 674
 675
 676     // Set up DFR to flat delivery mode.
 677     HAL_APIC_WRITE( HAL_APIC_DFR, 0xffffffff );
 678
 679     // Set up logical destination id. We set bit 1<<cpuid in the LDR
 680     // register.
 681
 682     HAL_APIC_READ( HAL_APIC_LDR, val );
 683     val |= 1<<(cpu+24);
 684     HAL_APIC_WRITE( HAL_APIC_LDR, val );
 685
 686     // Set TPR register to accept all.
 687     HAL_APIC_WRITE( HAL_APIC_TPR, 0 );
 688
 689     // Enable APIC in SPIV
 690     HAL_APIC_WRITE( HAL_APIC_SPIV, 0x00000100 );
 691
 692
 693     if( cyg_hal_smp_cpu_flags[HAL_SMP_CPU_THIS()] & 2 )
 694     {
 695         // This is the boot CPU, switch its PIC into APIC mode
 696         // Non-boot CPUs are already in APIC mode.
 697
 698         HAL_WRITE_UINT8( 0x22, 0x70 );
 699         HAL_WRITE_UINT8( 0x23, 0x01 );
 700     }
 701
 702     return 1;
 703 }
 704
 705 /*------------------------------------------------------------------------*/
 706 // Initialize I/O APIC
 707
 708 static cyg_bool cyg_hal_smp_init_ioapic(void)
 709 {
 710     CYG_WORD32 val;
 711     cyg_uint32 tabsize = 0;
 712     int i;
 713     HAL_SMP_CPU_TYPE cpu_this = HAL_SMP_CPU_THIS();
 714
 715     HAL_SPINLOCK_CLEAR( cyg_hal_ioapic_lock );
 716     HAL_SPINLOCK_SPIN( cyg_hal_ioapic_lock );
 717
 718     HAL_IOAPIC_READ( HAL_IOAPIC_REG_APICVER, val );
 719     tabsize = (val>>16)&0xFF;
 720
 721     // Set up ISA interrupts
 722     for( i = 0; i < 16; i++ )
 723     {
 724         if( cyg_hal_isa_bus_irq[i] != 100 )
 725         {
 726             CYG_WORD32 tehi = 0, telo = 0x00010000;
 727
 728             tehi |= cpu_this<<24;
 729
 730             telo |= CYGNUM_HAL_ISR_MIN+i;
 731
 732             HAL_IOAPIC_WRITE( HAL_IOAPIC_REG_REDIR_LO(cyg_hal_isa_bus_irq[i]), telo );
 733             HAL_IOAPIC_WRITE( HAL_IOAPIC_REG_REDIR_HI(cyg_hal_isa_bus_irq[i]), tehi );
 734         }
 735     }
 736
 737
 738 #if SHOW_DIAGNOSTICS
 739     diag_printf("I/O APIC: %08x\n",cyg_hal_smp_io_apic);
 740
 741     HAL_IOAPIC_READ( HAL_IOAPIC_REG_APICID, val );
 742     diag_printf("        ID: %08x\n",val);
 743
 744     HAL_IOAPIC_READ( HAL_IOAPIC_REG_APICVER, val );
 745     diag_printf("        VER: %08x\n",val);
 746
 747     HAL_IOAPIC_READ( HAL_IOAPIC_REG_APICARB, val );
 748     diag_printf("        ARB: %08x\n",val);
 749
 750     diag_printf("        Redirection Table:\n");
 751     for( i = 0; i < tabsize; i++ )
 752     {
 753         CYG_WORD32 tehi, telo;
 754
 755         HAL_IOAPIC_READ( HAL_IOAPIC_REG_REDIR_LO(i), telo );
 756         HAL_IOAPIC_READ( HAL_IOAPIC_REG_REDIR_HI(i), tehi );
 757         diag_printf("            %02d: %08x %08x\n",i,tehi,telo);
 758     }
 759 #endif
 760
 761     HAL_SPINLOCK_CLEAR( cyg_hal_ioapic_lock );
 762
 763     return 1;
 764 }
 765
 766 /*------------------------------------------------------------------------*/
 767
 768 static volatile CYG_WORD32 init_deasserted;
 769
 770 __externC volatile CYG_WORD32 cyg_hal_smp_cpu_sync_flag[HAL_SMP_CPU_MAX];
 771 __externC volatile CYG_WORD32 cyg_hal_smp_cpu_sync[HAL_SMP_CPU_MAX];
 772 __externC volatile void (*cyg_hal_smp_cpu_entry[HAL_SMP_CPU_MAX])(void);
 773 __externC volatile CYG_WORD32 cyg_hal_smp_vsr_sync_flag;
 774 __externC volatile CYG_WORD32 cyg_hal_smp_cpu_running[HAL_SMP_CPU_MAX];
 775
 776 /*------------------------------------------------------------------------*/
 777
 778 __externC void cyg_hal_smp_start(void);
 779
 780 __externC CYG_BYTE cyg_hal_slave_trampoline[];
 781 __externC CYG_BYTE cyg_hal_slave_trampoline_end[];
 782
 783 #define HAL_SLAVE_START_ADDRESS 0x00002000
 784
 785 __externC void cyg_hal_cpu_start( HAL_SMP_CPU_TYPE cpu )
 786 {
 787 #if 1
 788     CYG_WORD32 icrlo, icrhi;
 789     CYG_BYTE *p = &cyg_hal_slave_trampoline[0];
 790     CYG_BYTE *q = (CYG_BYTE *)HAL_SLAVE_START_ADDRESS;
 791     CYG_BYTE old_cmos;
 792     int i;
 793
 794     // Copy the trampoline over...
 795     do
 796     {
 797         *q++ = *p++;
 798     } while( p != &cyg_hal_slave_trampoline_end[0]);
 799
 800     // Init synchronization spinlock to locked to halt slave CPU in
 801     // cyg_hal_smp_startup().
 802
 803     init_deasserted = 0;
 804
 805     // We now have to execute the grungy and unpleasant AP startup
 806     // sequence to get the cpu running. I'm sure that not all of this
 807     // is strictly necessary, but it works and it is too much effort
 808     // to work out the minimal subset.
 809
 810     // Write warm-reset code into CMOS RAM and write the address of
 811     // the trampoline entry point into location 40:67.
 812     HAL_READ_CMOS( 0x0f, old_cmos );
 813     HAL_WRITE_CMOS( 0x0f, 0x0a );
 814     HAL_WRITEMEM_UINT16( 0x467, HAL_SLAVE_START_ADDRESS & 0xf );
 815     HAL_WRITEMEM_UINT16( 0x469, HAL_SLAVE_START_ADDRESS>>4 );
 816
 817
 818     // Send an INIT interrupt to the dest CPU
 819     icrhi = cpu<<24;
 820     icrlo = 0x0000C500;
 821
 822     HAL_APIC_WRITE( HAL_APIC_ICR_HI, icrhi );
 823     HAL_APIC_WRITE( HAL_APIC_ICR_LO, icrlo );
 824
 825     hal_delay_us( 10 * 1000 );  // Wait 10ms
 826
 827     // Wait for the ICR to become inactive
 828     do {
 829         HAL_APIC_READ( HAL_APIC_ICR_LO, icrlo );
 830     } while( (icrlo & 0x00001000) != 0 );
 831
 832     // Now de-assert INIT
 833
 834     icrlo = 0x00008500;
 835
 836     HAL_APIC_WRITE( HAL_APIC_ICR_HI, icrhi );
 837     HAL_APIC_WRITE( HAL_APIC_ICR_LO, icrlo );
 838
 839     init_deasserted = 1;
 840
 841     // Now we send two STARTUP IPIs
 842
 843     for( i = 0; i < 2; i++ )
 844     {
 845         icrlo = 0x00000600 | (HAL_SLAVE_START_ADDRESS>>12);
 846
 847         // Send the STARTUP IPI
 848         HAL_APIC_WRITE( HAL_APIC_ICR_HI, icrhi );
 849         HAL_APIC_WRITE( HAL_APIC_ICR_LO, icrlo );
 850
 851         hal_delay_us( 300 );
 852
 853         // Wait for the ICR to become inactive
 854         do {
 855             HAL_APIC_READ( HAL_APIC_ICR_LO, icrlo );
 856         } while( (icrlo & 0x00001000) != 0 );
 857
 858         hal_delay_us( 300 );
 859     }
 860
 861     HAL_WRITE_CMOS( 0x0f, old_cmos );
 862
 863 //    PC_WRITE_SCREEN( PC_SCREEN_LINE(5)+0, '!' );
 864
 865     hal_delay_us( 300 );
 866
 867 //    PC_WRITE_SCREEN( PC_SCREEN_LINE(5)+1, '!' );
 868
 869 #endif
 870 }
 871
 872 /*------------------------------------------------------------------------*/
 873
 874 __externC void cyg_hal_smp_start(void);
 875 __externC void cyg_hal_smp_startup(void);
 876
 877 __externC void cyg_hal_cpu_release( HAL_SMP_CPU_TYPE cpu )
 878 {
 879 //    PC_WRITE_SCREEN( PC_SCREEN_LINE(13), '!' );
 880 //    PC_WRITE_SCREEN_8( PC_SCREEN_LINE(13), cpu );
 881
 882     cyg_hal_smp_cpu_entry[cpu] = cyg_hal_smp_start;
 883
 884     while( cyg_hal_smp_cpu_entry[cpu] != 0 )
 885     {
 886 //        PC_WRITE_SCREEN_32( PC_SCREEN_LINE(13)+4, cyg_hal_smp_cpu_entry[cpu] );
 887         hal_delay_us( 100 );
 888          continue;
 889     }
 890 }
 891
 892 /*------------------------------------------------------------------------*/
 893
 894 __externC void cyg_hal_smp_startup(void)
 895 {
 896     HAL_SMP_CPU_TYPE cpu;
 897
 898 //    PC_WRITE_SCREEN( PC_SCREEN_LINE(2)+0, '!' );
 899
 900 #ifndef CYG_HAL_STARTUP_RAM
 901     // Wait for INIT interrupt to be deasserted
 902     while( !init_deasserted )
 903         continue;
 904 #endif
 905
 906 //    PC_WRITE_SCREEN( PC_SCREEN_LINE(2)+1, '!' );
 907
 908     cpu  = HAL_SMP_CPU_THIS();
 909
 910 //    PC_WRITE_SCREEN_8( PC_SCREEN_LINE(2)+6, cpu );
 911
 912 #ifndef CYG_HAL_STARTUP_RAM
 913     // Wait 1s for the world to settle
 914     hal_delay_us( 1000000 );
 915
 916 //    PC_WRITE_SCREEN( PC_SCREEN_LINE(2)+2, '!' );
 917
 918     // Setup our APIC
 919     cyg_hal_smp_init_apic();
 920 #endif
 921
 922 //    PC_WRITE_SCREEN( PC_SCREEN_LINE(2)+3, '!' );
 923
 924 #ifdef CYGPKG_KERNEL_SMP_SUPPORT
 925     cyg_hal_smp_cpu_running[cpu] = 1;
 926     cyg_kernel_smp_startup();
 927 #else
 928     for(;;)
 929     {
 930         void (*entry)(void);
 931
 932         while( (entry = cyg_hal_smp_cpu_entry[cpu]) == 0 )
 933         {
 934 #if 0 //SCREEN_DIAGNOSTICS
 935             static int n;
 936             PC_WRITE_SCREEN_8( PC_SCREEN_LINE(2)+10, n );
 937             PC_WRITE_SCREEN_8( PC_SCREEN_LINE(2)+15, cyg_hal_smp_cpu_sync[cpu] );
 938             PC_WRITE_SCREEN_8( PC_SCREEN_LINE(2)+30, cyg_hal_smp_cpu_sync_flag[0] );
 939             PC_WRITE_SCREEN_8( PC_SCREEN_LINE(2)+35, cyg_hal_smp_cpu_sync_flag[1] );
 940             PC_WRITE_SCREEN_8( PC_SCREEN_LINE(2)+40, cyg_hal_smp_vsr_sync_flag );
 941             n++;
 942 #endif
 943             hal_delay_us( 100 );
 944         }
 945
 946 //        PC_WRITE_SCREEN( PC_SCREEN_LINE(2)+4, '!' );
 947
 948         cyg_hal_smp_cpu_entry[cpu] = 0;
 949
 950 //        PC_WRITE_SCREEN_32( PC_SCREEN_LINE(2)+20, entry );
 951
 952         if( entry != NULL )
 953         {
 954             cyg_hal_smp_cpu_running[cpu] = 1;
 955             entry();
 956         }
 957     }
 958 #endif
 959 }
 960
 961 /*------------------------------------------------------------------------*/
 962
 963 __externC void cyg_hal_smp_init(void)
 964 {
 965     if( !cyg_hal_find_smp_config() )
 966         return;
 967
 968     if( !cyg_hal_parse_smp_config() )
 969         return;
 970
 971     if( !cyg_hal_smp_init_apic() )
 972         return;
 973
 974     if( !cyg_hal_smp_init_ioapic() )
 975         return;
 976 }
 977
 978 /*------------------------------------------------------------------------*/
 979
 980 __externC void cyg_hal_smp_cpu_start_all(void)
 981 {
 982     HAL_SMP_CPU_TYPE cpu;
 983
 984     for( cpu = 0; cpu < HAL_SMP_CPU_COUNT(); cpu++ )
 985     {
 986         cyg_hal_smp_cpu_sync[cpu] = 0;
 987         cyg_hal_smp_cpu_sync_flag[cpu] = 0;
 988         cyg_hal_smp_cpu_running[cpu] = 0;
 989         cyg_hal_smp_cpu_entry[cpu] = 0;
 990
 991         if( cpu != HAL_SMP_CPU_THIS() )
 992             cyg_hal_cpu_start( cpu );
 993         else cyg_hal_smp_cpu_running[cpu] = 1;
 994     }
 995 }
 996
 997 /*------------------------------------------------------------------------*/
 998 // SMP message buffers.
 999 // SMP CPUs pass messages to eachother via a small circular buffer
1000 // protected by a spinlock. Each message is a single 32 bit word with
1001 // a type code in the top 4 bits and any argument in the remaining
1002 // 28 bits.
1003
1004 #define SMP_MSGBUF_SIZE 4
1005
1006 static struct smp_msg_t
1007 {
1008     HAL_SPINLOCK_TYPE           lock;           // protecting spinlock
1009     volatile CYG_WORD32         msgs[SMP_MSGBUF_SIZE]; // message buffer
1010     volatile CYG_WORD32         head;           // head of list
1011     volatile CYG_WORD32         tail;           // tail of list
1012     volatile CYG_WORD32         reschedule;     // reschedule request
1013     volatile CYG_WORD32         timeslice;      // timeslice request
1014 } smp_msg[HAL_SMP_CPU_MAX];
1015
1016 /*------------------------------------------------------------------------*/
1017 // Pass a message to another CPU.
1018
1019 #if SCREEN_DIAGNOSTICS
1020 static int res_msgs[2], tms_msgs[2];
1021 #endif
1022
1023 __externC void cyg_hal_cpu_message( HAL_SMP_CPU_TYPE cpu,
1024                                     CYG_WORD32 msg,
1025                                     CYG_WORD32 arg,
1026                                     CYG_WORD32 wait)
1027 {
1028 #if 1
1029     CYG_INTERRUPT_STATE istate;
1030     struct smp_msg_t *m = &smp_msg[cpu];
1031     int i;
1032     HAL_SMP_CPU_TYPE me = HAL_SMP_CPU_THIS();
1033
1034     HAL_DISABLE_INTERRUPTS( istate );
1035
1036     // Get access to the message buffer for the selected CPU
1037     HAL_SPINLOCK_SPIN( m->lock );
1038
1039 #if 0 //SCREEN_DIAGNOSTICS
1040     if( msg == HAL_SMP_MESSAGE_RESCHEDULE )
1041         res_msgs[me]++;
1042     else if( msg == HAL_SMP_MESSAGE_TIMESLICE )
1043         tms_msgs[me]++;
1044     PC_WRITE_SCREEN_8( PC_SCREEN_LINE(18+me), me);
1045     PC_WRITE_SCREEN_16( PC_SCREEN_LINE(18+me)+40, res_msgs[me]);
1046     PC_WRITE_SCREEN_16( PC_SCREEN_LINE(18+me)+45, tms_msgs[me]);
1047 #endif
1048
1049     if( msg == HAL_SMP_MESSAGE_RESCHEDULE )
1050         m->reschedule = true;
1051     else if( msg == HAL_SMP_MESSAGE_TIMESLICE )
1052         m->timeslice = true;
1053     else
1054     {
1055         CYG_WORD32 next = (m->tail + 1) & (SMP_MSGBUF_SIZE-1);
1056
1057         // If the buffer is full, wait for space to appear in it.
1058         // This should only need to be done very rarely.
1059
1060         while( next == m->head )
1061         {
1062             HAL_SPINLOCK_CLEAR( m->lock );
1063             for( i = 0; i < 1000; i++ );
1064             HAL_SPINLOCK_SPIN( m->lock );
1065         }
1066
1067         m->msgs[m->tail] = msg | arg;
1068
1069         m->tail = next;
1070     }
1071
1072     // Now send an interrupt to the CPU.
1073
1074 //    PC_WRITE_SCREEN_16( PC_SCREEN_LINE(18+me)+50, cyg_hal_smp_cpu_running[cpu] );
1075
1076     if( cyg_hal_smp_cpu_running[cpu] )
1077     {
1078         CYG_WORD32 icrlo, icrhi;
1079
1080         // Set the ICR fields we want to write. Most fields are zero
1081         // except the destination in the high word and the vector
1082         // number in the low.
1083         icrhi = cpu<<24;
1084         icrlo = CYGNUM_HAL_SMP_CPU_INTERRUPT_VECTOR( cpu );
1085
1086         // Write the ICR register. The interrupt will be raised when
1087         // the low word is written.
1088         HAL_APIC_WRITE( HAL_APIC_ICR_HI, icrhi );
1089         HAL_APIC_WRITE( HAL_APIC_ICR_LO, icrlo );
1090
1091         // Wait for the ICR to become inactive
1092         do {
1093 #if 0 //SCREEN_DIAGNOSTICS
1094             static int n;
1095             PC_WRITE_SCREEN_8( PC_SCREEN_LINE(18+me)+55, n );
1096             n++;
1097 #endif
1098             HAL_APIC_READ( HAL_APIC_ICR_LO, icrlo );
1099         } while( (icrlo & 0x00001000) != 0 );
1100     }
1101
1102     HAL_SPINLOCK_CLEAR( m->lock );
1103
1104     // If we are expected to wait for the command to complete, then
1105     // spin here until it does. We actually wait for the destination
1106     // CPU to empty its input buffer. So we might wait for messages
1107     // from other CPUs as well. But this is benign.
1108
1109     while(wait)
1110     {
1111         for( i = 0; i < 1000; i++ );
1112
1113         HAL_SPINLOCK_SPIN( m->lock );
1114
1115         if( m->head == m->tail )
1116             wait = false;
1117
1118         HAL_SPINLOCK_CLEAR( m->lock );
1119
1120     }
1121
1122     HAL_RESTORE_INTERRUPTS( istate );
1123 #endif
1124 }
1125
1126 /*------------------------------------------------------------------------*/
1127
1128 #if SCREEN_DIAGNOSTICS
1129 static int isrs[2];
1130 static int dsrs[2];
1131 #endif
1132
1133 __externC CYG_WORD32 cyg_hal_cpu_message_isr( CYG_WORD32 vector, CYG_ADDRWORD data )
1134 {
1135     HAL_SMP_CPU_TYPE me = HAL_SMP_CPU_THIS();
1136     struct smp_msg_t *m = &smp_msg[me];
1137     CYG_WORD32 ret = 1;
1138     CYG_INTERRUPT_STATE istate;
1139
1140     HAL_DISABLE_INTERRUPTS( istate );
1141
1142     HAL_SPINLOCK_SPIN( m->lock );
1143
1144     // First, acknowledge the interrupt.
1145
1146     HAL_INTERRUPT_ACKNOWLEDGE( vector );
1147
1148 #if SCREEN_DIAGNOSTICS
1149     isrs[me]++;
1150     PC_WRITE_SCREEN_8( PC_SCREEN_LINE(18+me), me);
1151     PC_WRITE_SCREEN_16( PC_SCREEN_LINE(18+me)+5, isrs[me]);
1152 #endif
1153
1154     if( m->reschedule || m->timeslice )
1155         ret |= 2;               // Ask for the DSR to be called.
1156
1157     // Now pick messages out of the buffer and handle them
1158
1159     while( m->head != m->tail )
1160     {
1161         CYG_WORD32 msg = m->msgs[m->head];
1162
1163         switch( msg & HAL_SMP_MESSAGE_TYPE )
1164         {
1165         case HAL_SMP_MESSAGE_RESCHEDULE:
1166             ret |= 2;           // Ask for the DSR to be called.
1167             break;
1168         case HAL_SMP_MESSAGE_MASK:
1169             // Mask the supplied vector
1170 //            cyg_hal_interrupt_set_mask( msg&HAL_SMP_MESSAGE_ARG, false );
1171             break;
1172         case HAL_SMP_MESSAGE_UNMASK:
1173             // Unmask the supplied vector
1174 //            cyg_hal_interrupt_set_mask( msg&HAL_SMP_MESSAGE_ARG, true );
1175             break;
1176         case HAL_SMP_MESSAGE_REVECTOR:
1177             // Deal with a change of CPU assignment for a vector. We
1178             // only actually worry about what happens when the vector
1179             // is changed to some other CPU. We just mask the
1180             // interrupt locally.
1181 //            if( hal_interrupt_cpu[msg&HAL_SMP_MESSAGE_ARG] != me )
1182 //                cyg_hal_interrupt_set_mask( msg&HAL_SMP_MESSAGE_ARG, false );
1183             break;
1184         }
1185
1186         // Update the head pointer after handling the message, so that
1187         // the wait in cyg_hal_cpu_message() completes after the action
1188         // requested.
1189
1190         m->head = (m->head + 1) & (SMP_MSGBUF_SIZE-1);
1191     }
1192
1193     HAL_SPINLOCK_CLEAR( m->lock );
1194
1195     HAL_RESTORE_INTERRUPTS( istate );
1196
1197     return ret;
1198 }
1199
1200 /*------------------------------------------------------------------------*/
1201 // CPU message DSR.
1202 // This is only executed if the message was
1203 // HAL_SMP_MESSAGE_RESCHEDULE. It calls up into the kernel to effect a
1204 // reschedule.
1205
1206 __externC void cyg_scheduler_set_need_reschedule(void);
1207 __externC void cyg_scheduler_timeslice_cpu(void);
1208
1209 #if SCREEN_DIAGNOSTICS
1210 __externC int cyg_scheduler_sched_lock;
1211 static int rescheds[2];
1212 static int timeslices[2];
1213 #endif
1214
1215 __externC CYG_WORD32 cyg_hal_cpu_message_dsr( CYG_WORD32 vector, CYG_ADDRWORD data )
1216 {
1217     HAL_SMP_CPU_TYPE me = HAL_SMP_CPU_THIS();
1218     struct smp_msg_t *m = &smp_msg[me];
1219     CYG_INTERRUPT_STATE istate;
1220     CYG_WORD32 reschedule, timeslice;
1221
1222     HAL_DISABLE_INTERRUPTS( istate );
1223     HAL_SPINLOCK_SPIN( m->lock );
1224
1225 #if SCREEN_DIAGNOSTICS
1226     dsrs[me]++;
1227     PC_WRITE_SCREEN_16( PC_SCREEN_LINE(18+me)+10, dsrs[me]);
1228     PC_WRITE_SCREEN_16( PC_SCREEN_LINE(18+me)+15, cyg_scheduler_sched_lock);
1229 #endif
1230
1231     reschedule = m->reschedule;
1232     timeslice = m->timeslice;
1233     m->reschedule = m->timeslice = false;
1234
1235     HAL_SPINLOCK_CLEAR( m->lock );
1236     HAL_RESTORE_INTERRUPTS( istate );
1237
1238     if( reschedule )
1239     {
1240 #if SCREEN_DIAGNOSTICS
1241         rescheds[me]++;
1242         PC_WRITE_SCREEN_16( PC_SCREEN_LINE(18+me)+20, rescheds[me]);
1243 #endif
1244         cyg_scheduler_set_need_reschedule();
1245     }
1246     if( timeslice )
1247     {
1248 #if SCREEN_DIAGNOSTICS
1249         timeslices[me]++;
1250         PC_WRITE_SCREEN_16( PC_SCREEN_LINE(18+me)+25, timeslices[me]);
1251 #endif
1252         cyg_scheduler_timeslice_cpu();
1253     }
1254
1255     return 0;
1256
1257 }
1258
1259 /*------------------------------------------------------------------------*/
1260
1261 #if SCREEN_DIAGNOSTICS
1262 static int x = 0;
1263 #endif
1264
1265 __externC void cyg_hal_smp_halt_other_cpus(void)
1266 {
1267     int i;
1268     HAL_SMP_CPU_TYPE me = HAL_SMP_CPU_THIS();
1269
1270 //    PC_WRITE_SCREEN_8( PC_SCREEN_LINE(6+me), me );
1271
1272     for( i = 0 ; i < HAL_SMP_CPU_COUNT(); i++ )
1273     {
1274         if( i != me && cyg_hal_smp_cpu_running[i] )
1275         {
1276             CYG_WORD32 icrhi, icrlo;
1277             CYG_WORD32 oldsync;
1278
1279 //            PC_WRITE_SCREEN_8( PC_SCREEN_LINE(6+me)+40, i );
1280
1281             oldsync = cyg_hal_smp_cpu_sync_flag[i];
1282             cyg_hal_smp_cpu_sync[i] = 0;
1283
1284
1285             icrhi = i<<24;
1286             icrlo = CYGNUM_HAL_VECTOR_NMI;  // not really used
1287             icrlo |= 0x00000400;    // Delivery = NMI
1288             //icrlo |= 0x000C0000;    // Dest = all excluding self
1289
1290             // Write the ICR register. The interrupt will be raised when
1291             // the low word is written.
1292             HAL_APIC_WRITE( HAL_APIC_ICR_HI, icrhi );
1293             HAL_APIC_WRITE( HAL_APIC_ICR_LO, icrlo );
1294
1295             // Wait for the ICR to become inactive
1296             do {
1297 #if 0 //SCREEN_DIAGNOSTICS
1298                 static int n;
1299                 PC_WRITE_SCREEN_8( PC_SCREEN_LINE(6+me)+45, n );
1300                 n++;
1301 #endif
1302                 HAL_APIC_READ( HAL_APIC_ICR_LO, icrlo );
1303             } while( (icrlo & 0x00001000) != 0 );
1304
1305             // Wait for CPU to halt
1306             while( cyg_hal_smp_cpu_sync_flag[i] == oldsync )
1307             {
1308 #if 0 //SCREEN_DIAGNOSTICS
1309                 PC_WRITE_SCREEN_8( PC_SCREEN_LINE(6+me)+4, x ); x++;
1310                 PC_WRITE_SCREEN_8( PC_SCREEN_LINE(6+me)+10+(i*8), cyg_hal_smp_cpu_sync_flag[i] );
1311                 PC_WRITE_SCREEN_8( PC_SCREEN_LINE(6+me)+10+(i*8)+4, oldsync );
1312 #endif
1313                 hal_delay_us( 100 );
1314             }
1315
1316         }
1317     }
1318
1319 }
1320
1321 __externC void cyg_hal_smp_release_other_cpus(void)
1322 {
1323     int i;
1324     for( i = 0 ; i < HAL_SMP_CPU_COUNT(); i++ )
1325     {
1326         if( i != HAL_SMP_CPU_THIS() && cyg_hal_smp_cpu_running[i] )
1327         {
1328             CYG_WORD32 oldsync = cyg_hal_smp_cpu_sync_flag[i];
1329             cyg_hal_smp_cpu_sync[i] = 1;
1330             while( cyg_hal_smp_cpu_sync_flag[i] == oldsync )
1331                 continue;
1332             cyg_hal_smp_cpu_sync[i] = 0;
1333         }
1334     }
1335 }
1336
1337 #endif // CYGPKG_HAL_SMP_SUPPORT
1338
1339 /*------------------------------------------------------------------------*/
1340 /* End of pcmb_smp.c                                                      */