/* Reads the value of the semi-hosting vector. */    int debugID, OSinfo1, OSinfo2, subreason;    word semihosting_armswi, status;    IGNORE(stateptr);    unpack_message(BUFFERDATA(*buffer) + 4, "%w%w%w%w", &debugID, &OSinfo1,                   &OSinfo2, &subreason);    status = angelOS_SemiHosting_GetARMSWI(OSinfo1, OSinfo2,                                           &semihosting_armswi);    LogInfo(LOG_CTRL, ( "ctrl_semihosting_getarmswi -> %08x\n", semihosting_armswi));    return msgsend(CI_HADP, "%w%w%w%w%w%w%w",                   (ADP_Control | TtoH), debugID, OSinfo1, OSinfo2,                   ADP_Ctrl_SemiHosting_GetARMSWI, status, semihosting_armswi);#else    IGNORE(buffer);    IGNORE(stateptr);    LogInfo(LOG_CTRL, ( "ctrl_semihosting_getarmswi - unsupported\n"));    return -1;#endif}static int ctrl_semihosting_setthumbswi(p_Buffer * buffer, void *stateptr){#ifdef ICEMAN2    /* Set the semi-hosting ARM SWI. */    int debugID, OSinfo1, OSinfo2, subreason;    word semihosting_thumbswi, status;    IGNORE(stateptr);    unpack_message(BUFFERDATA(*buffer) + 4, "%w%w%w%w%w", &debugID, &OSinfo1,                   &OSinfo2, &subreason, &semihosting_thumbswi);    LogInfo(LOG_CTRL, ( "ctrl_semihosting_setthumbswi(swi = 0x%x)\n",                          semihosting_thumbswi));    status = angelOS_SemiHosting_SetThumbSWI(OSinfo1, OSinfo2,                                             semihosting_thumbswi);    return msgsend(CI_HADP, "%w%w%w%w%w%w",                   (ADP_Control | TtoH), debugID, OSinfo1, OSinfo2,                   ADP_Ctrl_SemiHosting_SetThumbSWI, status);#else    IGNORE(buffer);    IGNORE(stateptr);    LogInfo(LOG_CTRL, ( "ctrl_semihosting_setthumbswi -> unsupported\n"));    return -1;#endif}static int ctrl_semihosting_getthumbswi(p_Buffer * buffer, void *stateptr){#ifdef ICEMAN2    /* Reads the value of the semi-hosting vector. */    int debugID, OSinfo1, OSinfo2, subreason;    word semihosting_thumbswi, status;    IGNORE(stateptr);    unpack_message(BUFFERDATA(*buffer) + 4, "%w%w%w%w", &debugID, &OSinfo1,                   &OSinfo2, &subreason);    status = angelOS_SemiHosting_GetThumbSWI(OSinfo1, OSinfo2,                                             &semihosting_thumbswi);    LogInfo(LOG_CTRL, ("ctrl_semihosting_getthumbswi -> %08x\n",                         semihosting_thumbswi));    return msgsend(CI_HADP, "%w%w%w%w%w%w%w",                   (ADP_Control | TtoH), debugID, OSinfo1, OSinfo2,                   ADP_Ctrl_SemiHosting_GetThumbSWI,                   status, semihosting_thumbswi);#else    IGNORE(buffer);    IGNORE(stateptr);    LogInfo(LOG_CTRL, ( "ctrl_semihosting_getthumbswi - unsupported\n"));    return -1;#endif}static int ctrl_download_supported(p_Buffer * buffer, void *stateptr){    int debugID, OSinfo1, OSinfo2;    /* For Angel return CantLoadConfig - we can't do that     */    word status = RDIError_CantLoadConfig;    IGNORE(stateptr);    LogInfo(LOG_CTRL, ( "ctrl_download_supported -> %d\n", status));    unpack_message(BUFFERDATA(*buffer) + 4, "%w%w%w", &debugID, &OSinfo1,                   &OSinfo2);    return msgsend(CI_HADP, "%w%w%w%w%w%w", (ADP_Control | TtoH),                   debugID, OSinfo1, OSinfo2, ADP_Ctrl_Download_Supported,                   status);}static word loadagent_address = (word) - 1;static word loadagent_size;static word loadagent_sofar;static int ctrl_download_data(p_Buffer * buffer, void *stateptr){    /* Decode an addconfig message */    int debugID, OSinfo1, OSinfo2, reason, subreason, count, err;    word nbytes;    word status;    IGNORE(stateptr);    count = unpack_message(BUFFERDATA(*buffer), "%w%w%w%w%w%w", &reason,                           &debugID, &OSinfo1, &OSinfo2, &subreason, &nbytes);    LogInfo(LOG_CTRL, ( "ctrl_download_data(nbytes = %d)\n", nbytes));    status = angelOS_LoadConfigData(OSinfo1, OSinfo2,                                    nbytes, BUFFERDATA(*buffer) + count);    if (status == RDIError_NoError && loadagent_address != -1)        loadagent_sofar += nbytes;    /* We make sure we release the buffer here as it may be the long one */    angel_ChannelReleaseBuffer(*buffer);    *buffer = NULL;    err = msgsend(CI_HADP, "%w%w%w%w%w%w",                  (ADP_Control | TtoH), debugID, OSinfo1, OSinfo2,                  ADP_Ctrl_Download_Data, status);    return err;}static int ctrl_download_agent(p_Buffer * buffer, void *stateptr){    /* Decode an loadagent message */    int debugID, OSinfo1, OSinfo2, subreason;    word loadaddress, nbytes;    word status;    IGNORE(stateptr);    unpack_message(BUFFERDATA(*buffer) + 4, "%w%w%w%w%w%w", &debugID, &OSinfo1,                   &OSinfo2, &subreason, &loadaddress, &nbytes);        LogInfo(LOG_CTRL, ( "ctrl_download_agent(loadadr = 0x%x, nbytes = %d)\n",                          loadaddress, nbytes));    status = angelOS_LoadAgent(OSinfo1, OSinfo2, loadaddress, nbytes);    if (status == RDIError_NoError)    {        loadagent_address = loadaddress;        loadagent_size = nbytes;        loadagent_sofar = 0;    }    return msgsend(CI_HADP, "%w%w%w%w%w%w",                   (ADP_Control | TtoH), debugID, OSinfo1, OSinfo2,                   ADP_Ctrl_Download_Agent, status);}static int ctrl_start_agent(p_Buffer * buffer, void *stateptr){    /* Decode an loadagent message */    int debugID, OSinfo1, OSinfo2, subreason, err;    word startaddress;    word status;    IGNORE(stateptr);    unpack_message(BUFFERDATA(*buffer) + 4, "%w%w%w%w%w", &debugID, &OSinfo1,                   &OSinfo2, &subreason, &startaddress);    LogInfo(LOG_CTRL, ( "ctrl_start_agent(startaddress = %x)\n", startaddress));    if (loadagent_sofar == loadagent_size        && startaddress >= loadagent_address        && startaddress < (loadagent_address + loadagent_size))        status = RDIError_NoError;    else        status = RDIError_BadConfigData;    err = msgsend(CI_HADP, "%w%w%w%w%w%w",                  (ADP_Control | TtoH), debugID, OSinfo1, OSinfo2,                  ADP_Ctrl_Start_Agent, status);    if (err == RDIError_NoError && status == RDIError_NoError)        angelOS_ExecuteNewAgent(startaddress);    return err;}static int ctrl_settopmem(p_Buffer * buffer, void *stateptr){#ifdef ICEMAN2    /* Set the top of memory we report on a HEAPINFO SWI */    int debugID, OSinfo1, OSinfo2, subreason;    word topmem;    IGNORE(stateptr);    unpack_message(BUFFERDATA(*buffer) + 4, "%w%w%w%w%w", &debugID, &OSinfo1,                   &OSinfo2, &subreason, &topmem);    LogInfo(LOG_CTRL, ( "ctrl_settopmem(topmem = 0x%x)\n", topmem));    angel_SetTopMem((unsigned)topmem);    return msgsend(CI_HADP, "%w%w%w%w%w%w",                   (ADP_Control | TtoH), debugID, OSinfo1, OSinfo2,                   ADP_Ctrl_SetTopMem, RDIError_NoError);#else    IGNORE(buffer);    IGNORE(stateptr);    return -1;#endif}const handler_function_pointer ctrl_hfptr[] ={    ctrl_nop,    ctrl_vectorcatch,    ctrl_pointstatus_watch,    ctrl_pointstatus_break,    ctrl_semihosting_setstate,    ctrl_semihosting_getstate,    ctrl_semihosting_setvector,    ctrl_semihosting_getvector,    ctrl_log,    ctrl_setlog,    ctrl_semihosting_setarmswi,    ctrl_semihosting_getarmswi,    ctrl_semihosting_setthumbswi,    ctrl_semihosting_getthumbswi,    ctrl_download_supported,    ctrl_download_data,    ctrl_download_agent,    ctrl_start_agent,    ctrl_settopmem};const int ctrl_hfptr_max =sizeof(ctrl_hfptr) / sizeof(handler_function_pointer);/* end of file */