v86.c

该源码是AT91rm9200的U-boot原代码。

        /* WARNING: no test for _SI wrapping! */
        if (prefix66) {
            CPU_REG_LW(esi) += port_rep_outl(CPU_REG_LW(edx),
                                             (CARD32 *)LIN_PREF_SI,
                                             CPU_REG_LW(eflags)&DF,
                                             (is_rep? LWECX:1));
        }
        else {
            CPU_REG_LW(esi) += port_rep_outw(CPU_REG_LW(edx),
                                             (CARD16 *)LIN_PREF_SI,
                                             CPU_REG_LW(eflags)&DF,
                                             (is_rep? LWECX:1));
        } 
        if (is_rep) LWECX = 0;
        CPU_REG_LW(eip)++;
        break;

    case 0xe5:                  /* inw xx, inl xx */
        if (prefix66) CPU_REG(eax) = P.inl((int) csp[1]);
        else CPU_REG_LW(eax) = P.inw((int) csp[1]);  
        CPU_REG_LW(eip) += 2;
        break;
    case 0xe4:                  /* inb xx */
        CPU_REG_LW(eax) &= ~(CARD32)0xff;
        CPU_REG_LB(ax) |= P.inb((int) csp[1]);
        CPU_REG_LW(eip) += 2;
        break;
    case 0xed:                  /* inw dx, inl dx */
        if (prefix66) CPU_REG(eax) = P.inl(CPU_REG_LW(edx)); 
        else CPU_REG_LW(eax) = P.inw(CPU_REG_LW(edx));
        CPU_REG_LW(eip) += 1;
        break;
    case 0xec:                  /* inb dx */
        CPU_REG_LW(eax) &= ~(CARD32)0xff;
        CPU_REG_LB(ax) |= P.inb(CPU_REG_LW(edx));
        CPU_REG_LW(eip) += 1;
        break;

    case 0xe7:                  /* outw xx */
        if (prefix66) P.outl((int)csp[1], CPU_REG(eax));
        else P.outw((int)csp[1], CPU_REG_LW(eax));
        CPU_REG_LW(eip) += 2;
        break;
    case 0xe6:                  /* outb xx */
        P.outb((int) csp[1], CPU_REG_LB(ax));
        CPU_REG_LW(eip) += 2;
        break;
    case 0xef:                  /* outw dx */
        if (prefix66) P.outl(CPU_REG_LW(edx), CPU_REG(eax));
        else P.outw(CPU_REG_LW(edx), CPU_REG_LW(eax));
        CPU_REG_LW(eip) += 1;
        break;
    case 0xee:                  /* outb dx */
        P.outb(CPU_REG_LW(edx), CPU_REG_LB(ax));
        CPU_REG_LW(eip) += 1;
        break;

    case 0xf4:
#ifdef V86BIOS_DEBUG
        printf("hlt at %p\n", lina);
#endif
        return 0;

    case 0x0f: 
        fprintf(stderr,"CPU 0x0f Trap at eip=0x%lx\n",CPU_REG(eip));
        goto op0ferr; 
        break;

    case 0xf0:                  /* lock */
    default:
        fprintf(stderr,"unknown reason for exception\n");
        dump_registers();
        stack_trace();
    op0ferr:
        dump_code();
        fprintf(stderr,"cannot continue\n");
        return 0;
    }                           /* end of switch() */
    return 1;
}

static int
vm86_do_int(int num)
{
    int val;
    struct regs86 regs;

    i_printf("int 0x%x received: ax:0x%lx",num,CPU_REG(eax));
    if (Config.PrintIp)
        i_printf(" at: 0x%x\n",getIP());
    else
        i_printf("\n");

    /* try to run bios interrupt */
    
    /* if not installed fall back */
#define COPY(x) regs.##x = CPU_REG(x)
#define COPY_R(x) CPU_REG(x) = regs.##x
    
    COPY(eax);
    COPY(ebx);
    COPY(ecx);
    COPY(edx);
    COPY(esi);
    COPY(edi);
    COPY(ebp);
    COPY(eip);
    COPY(esp);
    COPY(cs);
    COPY(ss);
    COPY(ds);
    COPY(es);
    COPY(fs);
    COPY(gs);
    COPY(eflags);

    if (!(val = int_handler(num,&regs)))
        if (!(val = run_bios_int(num,&regs)))
            return val;
        
    COPY_R(eax);
    COPY_R(ebx);
    COPY_R(ecx);
    COPY_R(edx);
    COPY_R(esi);
    COPY_R(edi);
    COPY_R(ebp);
    COPY_R(eip);
    COPY_R(esp);
    COPY_R(cs);
    COPY_R(ss);
    COPY_R(ds);
    COPY_R(es);
    COPY_R(fs);
    COPY_R(gs);
    COPY_R(eflags);

    return val;
#undef COPY
#undef COPY_R
}

static void
dump_code(void)
{
    int i;
    unsigned char *lina = SEG_ADR((unsigned char *), cs, ip);

    fprintf(stderr,"code at 0x%8.8x: ",(CARD32)lina);
    for (i=0; i<0x10; i++) 
        fprintf(stderr,"%2.2x ",*(lina + i));
    fprintf(stderr,"\n                    ");
    for (; i<0x20; i++) 
        fprintf(stderr,"%2.2x ",*(lina + i));
    fprintf(stderr,"\n");
}

#define PRINT(x) fprintf(stderr,#x":%4.4x ",CPU_REG_LW(x))
#define PRINT_FLAGS(x) fprintf(stderr,#x":%8.8x ",CPU_REG_LW(x))
static void
dump_registers(void)
{
    PRINT(eip);
    PRINT(eax);
    PRINT(ebx);
    PRINT(ecx);
    PRINT(edx);
    PRINT(esi);
    PRINT(edi);
    PRINT(ebp);
    fprintf(stderr,"\n");
    PRINT(esp);
    PRINT(cs);
    PRINT(ss);
    PRINT(es);
    PRINT(ds);
    PRINT(fs);
    PRINT(gs);
    PRINT_FLAGS(eflags);
    fprintf(stderr,"\n");
}

static void
stack_trace(void)
{
    int i;
    unsigned char *stack = SEG_ADR((unsigned char *), ss, sp);    

    fprintf(stderr,"stack at 0x%8.8lx:\n",(unsigned long)stack);
    for (i=0; i < 0x10; i++) 
        fprintf(stderr,"%2.2x ",*(stack + i));
    fprintf(stderr,"\n");
    
}

static int
vm86_rep(struct vm86_struct *ptr) 
{

    int __res;

    __asm__ __volatile__("int $0x80\n"
                         :"=a" (__res):"a" ((int)113),
                         "b" ((struct vm86_struct *)ptr));

            if ((__res) < 0) {
                errno = -__res;
                __res=-1;
            }
            else errno = 0;
            return __res;
}

#define pushw(base, ptr, val) \
__asm__ __volatile__( \
        "decw %w0\n\t" \
        "movb %h2,(%1,%0)\n\t" \
        "decw %w0\n\t" \
        "movb %b2,(%1,%0)" \
        : "=r" (ptr) \
        : "r" (base), "q" (val), "0" (ptr))

int
run_bios_int(int num, struct regs86 *regs)
{
    CARD16 *ssp;
    CARD32 sp;
    CARD32 eflags;

#ifdef V86BIOS_DEBUG
    static int firsttime = 1;
#endif
    /* check if bios vector is initialized */
    if (((CARD16*)0)[(num<<1)+1] == 0x0000) { /* SYS_BIOS_SEG ?*/
#ifdef V86BIOS_DEBUG
        i_printf("card BIOS not loaded\n");
#endif
        return 0;
    }
    
#ifdef V86BIOS_DEBUG
    if (firsttime) {
        dprint(0,0x3D0);
        firsttime = 0;
    }
#endif
    
    i_printf("calling card BIOS at: ");
    ssp = (CARD16*)(CPU_REG(ss)<<4);
    sp = (CARD32) CPU_REG_LW(esp);

    eflags = regs->eflags;
    eflags = ((eflags & VIF_MASK) != 0)
        ? (eflags | IF_MASK) : (eflags & ~(CARD32) IF_MASK);
    pushw(ssp, sp, eflags);
    pushw(ssp, sp, regs->cs);
    pushw(ssp, sp, (CARD16)regs->eip);
    regs->esp -= 6;
    regs->cs = ((CARD16 *) 0)[(num << 1) + 1];
    regs->eip = (regs->eip & 0xFFFF0000) | ((CARD16 *) 0)[num << 1];
    i_printf("0x%x:%lx\n",regs->cs,regs->eip);
#ifdef V86BIOS_DEBUG
    dump_code();
#endif
    regs->eflags = regs->eflags
                       & ~(VIF_MASK | TF_MASK | IF_MASK | NT_MASK);
    return 1;
}

CARD32
getIntVect(int num)
{
    return ((CARD32*)0)[num];
}

CARD32
getIP(void)
{
    return (CPU_REG(cs) << 4) + CPU_REG(eip);
}