semihost.c

realview22.rar

    OS_State *osstate = (OS_State *)handle;
    void *data = (void*)arg1;
    (void)arg2;

#ifdef VERBOSE_OS_INFO
    Hostif_DebugPrint(osstate->hostif,"OS_HandleUnkRDI subcode:0x%08lx=%lu\n",
                      (unsigned long)subcode,(unsigned long)subcode);
#endif
    
    switch (subcode) {

    case RDISemiHosting_GetARMSWI:
        *arg1 = osstate->AngelSWIARM;
    case RDIInfo_SemiHostingGetARMSWI:
        return RDIError_NoError;
    case RDISemiHosting_SetARMSWI:
        osstate->AngelSWIARM = *arg1;
    case RDIInfo_SemiHostingSetARMSWI:
        return RDIError_NoError;
    case RDISemiHosting_GetThumbSWI:
        *arg1 = osstate->AngelSWIThumb;
    case RDIInfo_SemiHostingGetThumbSWI:
        return RDIError_NoError;
    case RDISemiHosting_SetThumbSWI:
        osstate->AngelSWIThumb = *arg1;
    case RDIInfo_SemiHostingSetThumbSWI:
        return RDIError_NoError;
    case RDISemiHosting_SetState: /*0x181*/
        osstate->semihostingstate = *arg1;
        return RDIError_NoError;
    case RDISemiHosting_GetState: /*0x182*/
        *arg1 = osstate->semihostingstate;
        return RDIError_NoError;
    case RDISemiHosting_SetVector: /*0x183*/
        osstate->semihostingvector = *arg1;
        return RDIError_NoError;
    case RDISemiHosting_GetVector: /*0x184*/
        *arg1 = osstate->semihostingvector;
        return RDIError_NoError;

    case RDIVector_Catch:
        osstate->vector_catch = *arg1;
#ifdef VERBOSE_VECTOR_CATCH
        printf("Semihost: RDIVectorCatch=0x%08x demon:%u\n",
               (unsigned)osstate->vector_catch,
               (unsigned)osstate->DemonEnabled);
#endif
        return (osstate->DemonEnabled) ?
            RDIError_NoError : /* Grab it! */
            RDIError_UnimplementedMessage; /* Let the core handle it */
    case RDIInfo_SemiHosting:
         return RDIError_NoError; /* Yes we do! */
    case RDISet_Cmdline :
#ifdef VERBOSE_CMDLINE
        printf("RDISet_Cmdline [%s]\n",(char *)data);
#endif
          if (osstate != NULL)
            free((char *)(osstate->CommandLine));
 
         osstate->CommandLine = (char *)
             calloc(1,(unsigned)strlen((char *)data) + 1);

         if (osstate->CommandLine != NULL)
             (void)strcpy(osstate->CommandLine, (char *)data);
         /*
          * XXXX We swallow this call - there can be only one OS (shame).
          */
         return RDIError_NoError;

    case RDIErrorP : /* return the last Operating System Error */
         if (osstate->ErrorNo != 0)
             *(ARMword *)data = osstate->ErrorNo;
         return RDIError_NoError;
    case RDIProperty_SetAsNumeric:
    {
        unsigned PrID = (unsigned)arg1;
        if (arg2 == NULL)
            return RDIError_UnimplementedMessage;
        switch (PrID)
        {
        case RDIPropID_ARMulProp_Hostif:
            osstate->hostif = *(RDI_HostosInterface const **)arg2;
#if VERBOSE_PROP_HOSTIF
            printf("\n** Semihost rcv RDIPropID_ARMulProp_Hostif **\n");
#endif
            return RDIError_UnimplementedMessage;
            /* Safer than return RDIError_NoError;
             * in case we have more than 1 listener.
             */
        default:
            return RDIError_UnimplementedMessage;
        }   
    }
    case RDIInfo_Target:
    {
        ARMword arg1val;
        arg1val = RDITarget_HostosSignalAware;  /* Emulator, speed 10**5 IPS */
        *arg1 |= arg1val;
        
        return RDIError_UnimplementedMessage;
    }
    case RDISignal_Stop:
        osstate->bStop_Received = TRUE;
        return RDIError_UnimplementedMessage;
   default:
       return RDIError_UnimplementedMessage;
   }
}

/***************************************************************************\
*                             Get a SWI argument                            *
\***************************************************************************/

static ARMword GetNthWordArg(OS_State *osstate,ARMword arg, unsigned isangel)
{
   ARMword mode = ARMulif_GetCPSR(&osstate->coredesc) & 0x1f ;
   if (isangel)
   {
      ARMword baseAddr = ARMulif_GetReg(&osstate->coredesc,mode,1);
      return ARMulif_ReadWord(&osstate->coredesc, baseAddr + (arg * 4));
   }
   else
      return ARMulif_GetReg(&osstate->coredesc,mode,arg);
}

static char GetNthByteArg(OS_State *osstate,ARMword arg, unsigned isangel)
   {
   ARMword mode = ARMulif_GetCPSR(&osstate->coredesc) & 0x1f ;
   if (isangel)
      return (char)ARMulif_ReadByte(&osstate->coredesc, ARMulif_GetReg(&osstate->coredesc,mode,1) + arg);
   else
      return (char)ARMulif_GetReg(&osstate->coredesc,mode,arg);
   }

/***************************************************************************\
*  Copy a null-terminated string from the debuggee's memory to the host's  *
\***************************************************************************/

static bool GetString(RDI_ModuleDesc *coredesc, ARMword from, char *to,
                      unsigned int BufferSize)
{
    unsigned int i = 0;
    do {
        if (++i > BufferSize) 
        {
            return FALSE;
        }
        *to = (char)ARMulif_ReadByte(coredesc,from++);
    } while (*to++ != '\0');
    return TRUE;
}



/* ---------- Demon Exception-Exit Stuff ------------ */

/*
 * Macros to extract instruction fields
 */

#define BIT(n) ( (ARMword)(instr>>(n))&1)   /* bit n of instruction */
#define BITS(m,n) ( (ARMword)(instr<<(31-(n))) >> ((31-(n))+(m)) ) /* bits m to n of instr */
#define TOPBITS(n) (instr >> (n)) /* bits 31 to n of instr */


#define UNEG(aval) (~aval + 1)
#define CPSRINSTRUCTIONSETBITPOSN (5)
#define CPSRINSTRUCTIONSETBITS (0x1 << CPSRINSTRUCTIONSETBITPOSN)
#define CPSRINSTRUCTIONSET(r) ((r & CPSRINSTRUCTIONSETBITS) ? INSTRUCTION16 : INSTRUCTION32)
#define CPSRINSTRUCTIONSIZE(r) ((r & CPSRINSTRUCTIONSETBITS) ? INSTRUCTION16SIZE : INSTRUCTION32SIZE)

/* state->Instruction_set values */
#define INSTRUCTION32 (0)
#define INSTRUCTION16 (1)
#define INSTRUCTION16SIZE 2
#define INSTRUCTION32SIZE 4

/*
 * Unwind a data abort
 */

static void UnwindDataAbort(OSState *osstate, ARMword addr, ARMword iset)
{
  if (iset == INSTRUCTION16) {
    ARMword instr = ARMulif_ReadWord(&osstate->coredesc, addr);
    ARMword offset;
    unsigned long regs;
    if (/*state->bigendSig */ 0 ^ ((addr & 2) == 2))
      /* get instruction into low 16 bits */
      instr = instr >> 16;
    switch (BITS(11,15)) {
    case 0x16:
    case 0x17:
      if (BITS(9,10) == 2) { /* push/pop */
        regs = BITS(0, 8);
        offset = 0;
        for (; regs != 0; offset++)
          regs ^= (regs & UNEG(regs));
        /* pop */
        if (BIT(11))
          ARMulif_SetReg(&osstate->coredesc, CURRENTMODE,13,
                       ARMulif_GetReg(&osstate->coredesc, CURRENTMODE,13) - offset*4);
        else
          ARMulif_SetReg(&osstate->coredesc, CURRENTMODE,13,
                       ARMulif_GetReg(&osstate->coredesc, CURRENTMODE,13) + offset*4);
      }
      break;
    case 0x18:
    case 0x19:
      regs = BITS(0,7);
      offset = 0;
      for (; regs != 0; offset++)
        regs ^= (regs & UNEG(regs));
      /* pop */
      if (BITS(11,15) == 0x19) { /* ldmia rb! */
        ARMulif_SetReg(&osstate->coredesc, CURRENTMODE,BITS(8,10),
                     ARMulif_GetReg(&osstate->coredesc, CURRENTMODE,BITS(8,10)) - offset*4);
      } else {          /* stmia rb! */
        ARMulif_SetReg(&osstate->coredesc, CURRENTMODE,BITS(8,10),
                     ARMulif_GetReg(&osstate->coredesc, CURRENTMODE,BITS(8,10)) + offset*4);
      }
      break;
    default:
      break;
    }
  } else
  {
    ARMword instr = ARMulif_ReadWord(&osstate->coredesc, addr);
    ARMword rn = BITS(16, 19);
    ARMword itype = BITS(24, 27);
    ARMword offset;

    if (rn == 15) return;
    if (itype == 8 || itype == 9) {
      /* LDM or STM */
      unsigned long regs = BITS(0, 15);
      offset = 0;
      if (!BIT(21)) return; /* no wb */
      for (; regs != 0; offset++)
        regs ^= (regs & UNEG(regs)); /* (regs & -regs) */
      if (offset == 0) offset = 16;
    } else if (itype == 12 || /* post-indexed CPDT */
               (itype == 13 && BIT(21))) { /* pre_indexed CPDT with WB */
      offset = BITS(0, 7);
    } else
      return;
    
    /* pop */
    if (BIT(23))
      ARMulif_SetReg(&osstate->coredesc,CURRENTMODE,rn,
                   ARMulif_GetReg(&osstate->coredesc,CURRENTMODE,rn) - offset*4);
    else
      ARMulif_SetReg(&osstate->coredesc,CURRENTMODE,rn,
                   ARMulif_GetReg(&osstate->coredesc,CURRENTMODE,rn) + offset*4);
  }
}



/*
 * If demon is enabled, we shall provide semihosting SWI's
 * >= 0x80 for the FPE, which shall implement vector_catch.
 *
 * A typical sequence for an unhandled coprocessor-access-bounce
 * is SWI 0x91 (raise undef) then SWI 0x81 (pass to debugger).
 *
 * A major issue is what value to write to R15.
 * If we halt "immediately" then we must add PCR15DIFFERENCE to the
 * value we write to R15, because this will be subtracted before
 * the debugger gets it.
 * If we halt after refilling the pipeline (as ARM7 + ARM9 cores do,
 * using state->NextInstr & PRIMEPIPE),
 * then we don't want an offset.
 * If we are in ARMISS, and always bias R15 on both reads and writes,
 * then we also don't want to add anything to R15.
 * This leaves StrongARM, which fortunately also does a pipe-refill
 * when an Abort is handled.
 */
static bool handleDemonExitSWIs(OSState *osstate, unsigned number)
{
  OSState *OSptr = osstate;
#ifdef VERBOSE_DEMON_EXIT
    printf("handleDemonExitSWIs enabled:%u number:0x%04x vector_catch:%08lx\n",
           (unsigned)osstate->DemonEnabled,(unsigned)number, 
           (unsigned long)OSptr->vector_catch);
#endif
  if (!osstate->ExitSwisEnabled)
    return FALSE;
  switch(number)
  {
  case SWI_InstallHandler: 
  {
    /*
     * SWI_InstallHandler installs a handler for a hardware exception. On
     * entry r0 contains the exception number; r1 contains a value to pass
     * to the handler when it is eventually invoked; and r2 contains the
     * address of the handler. On return r2 contains the adderss of the
     * previous handler and r1 contains its argument.
     */
    ARMword handlerp = (OSptr->map.handlers + 
                        ARMulif_GetReg(&osstate->coredesc,CURRENTMODE,0) * 8);
    ARMword oldr1 = ARMulif_ReadWord(&osstate->coredesc, handlerp),
    oldr2 = ARMulif_ReadWord(&osstate->coredesc, handlerp + 4);
    ARMulif_WriteWord(&osstate->coredesc, handlerp, 
                      ARMulif_GetReg(&osstate->coredesc,CURRENTMODE,1));
    ARMulif_WriteWord(&osstate->coredesc, handlerp + 4, 
                      ARMulif_GetReg(&osstate->coredesc,CURRENTMODE,2));
    ARMulif_SetReg(&osstate->coredesc,CURRENTMODE,1, oldr1);
    ARMulif_SetReg(&osstate->coredesc,CURRENTMODE,2, oldr2);
    return TRUE;
  }
  case 0x90: {          /* Branch through zero */
    ARMword oldpsr = ARMulif_GetCPSR(&osstate->coredesc);
    ARMulif_SetCPSR(&osstate->coredesc, (oldpsr & 0xffffffc0) | 0x13);
    ARMulif_SetSPSR(&osstate->coredesc, SVC32MODE, oldpsr);
    ARMulif_SetReg(&osstate->coredesc,CURRENTMODE,14, 0);
    goto TidyCommon;
  }
    
  case 0x98: {          /* Error */
    ARMword errorp = ARMulif_GetReg(&osstate->coredesc,CURRENTMODE,0),
    regp = ARMulif_GetReg(&osstate->coredesc,CURRENTMODE,1);
    unsigned i;
    ARMword errorpsr = ARMulif_ReadWord(&osstate->coredesc, regp + 16*4);
    for (i = 0; i < 15; i++)
      ARMulif_SetReg(&osstate->coredesc,(unsigned)errorpsr,i,
                   ARMulif_ReadWord(&osstate->coredesc, regp + i*4L));
    ARMulif_SetReg(&osstate->coredesc,CURRENTMODE,14, ARMulif_ReadWord(&osstate->coredesc, regp + 15*4L));
    ARMulif_SetReg(&osstate->coredesc,CURRENTMODE,10, errorp);
    ARMulif_SetSPSR(&osstate->coredesc,CURRENTMODE,errorpsr);
    OSptr->ErrorNo = errorp; /* ErrorP */
    goto TidyCommon;
  }
    
  case 0x94: {          /* Data abort */
        /* DATA ABORT RETURN ADDRESS FIXUP */
    ARMword addr = (ARMulif_GetReg(&osstate->coredesc,CURRENTMODE,14) - 8);
    ARMword cpsr = ARMulif_GetCPSR(&osstate->coredesc);
    /* QQ: Do we really still support 26-bit mode? I think not. */
    if (ARMulif_GetMode(&osstate->coredesc) < USER32MODE)
      addr = addr & 0x3fffffc;
    ARMulif_SetCPSR(&osstate->coredesc,ARMulif_GetSPSR(&osstate->coredesc,cpsr & 0x1f));
    UnwindDataAbort(osstate, addr,
                 CPSRINSTRUCTIONSET(ARMulif_GetSPSR(&osstate->coredesc,cpsr & 0x1f)));
    ARMulif_SetCPSR(&osstate->coredesc,cpsr);
    
    /* and fall through to correct r14 */
  } 
  case 0x95:                    /* Address Exception */
    ARMulif_SetReg(&osstate->coredesc,CURRENTMODE,14,
                 ARMulif_GetReg(&osstate->coredesc, CURRENTMODE, 14)-8);
  case 0x91:                    /* Undefined instruction */
  case 0x92:                    /* SWI */
  case 0x93:                    /* Prefetch abort */
  case 0x96:                    /* IRQ */
  case 0x97:                    /* FIQ */

 
    ARMulif_SetReg(&osstate->coredesc,CURRENTMODE,14,
                 ARMulif_GetReg(&osstate->coredesc, CURRENTMODE, 14)-4);
#ifdef VERBOSE_DEMON_EXIT
    printf("DemonExitSWI number:0x%04x just reduced R14 to %08lx\n",
           (unsigned)number, 
           (unsigned long)ARMulif_GetReg(&osstate->coredesc, CURRENTMODE, 14));
#endif
  TidyCommon:
    if (OSptr->vector_catch & (1 << (number - 0x90))) {
        /* DRS 2000-08-25
         * Lars agrees that we want to point at the instruction which
         * we are catching, and this makes armsd look right.
         * (OTOH, I'm not sure that we want to apply this
         */
      /* Point at the instruction that cause the vector-catch */
# define DEMON_PC_OFFSET 0

        /* This has no effect while stepping through an UNDEF that