mycopro.c

ARM 的实例程序

/* mycopro.c - ARM apps note 32 */
#include "minperip.h"
#include "armul_mem.h"
#include "armul_copro.h"

#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 */

/*
 * What follows is a copy of the validation Suite Coprocessor.  
 * It has the follwing functionality.
 * Sixteen registers.
 * co-processor can be used in an MCR and MRC to access
 *   these registers.
 * LDC and STC to and from the registers.
 * Will busy wait for the number of cycles specified by a CP register.
 * CDP 1 issues a FIQ after a number of cycles (specified in a CP
 *   register),
 * CDP 2 issues an IRQW in the same way, CDP 3 and 4 turn off the FIQ
 *   and IRQ source, and CDP 5 stores a 32 bit time value in a CP
 *   register (actually it's the total number of N, S, I, C and F
 *   cyles)

*/

/* copro regs */

static ARMword ValReg[16];

/* new prototypes follow NCAccessFunc declaration */

static int LDC( void *handle, int type, ARMword instr, uint32 *data );
static int STC( void *handle, int type, ARMword instr, uint32 *data );
static int MRC( void *handle, int type, ARMword instr, uint32 *data );
static int MCR( void *handle, int type, ARMword instr, uint32 *data );
static int CDP( void *handle, int type, ARMword instr, uint32 *data );

static unsigned DoAFIQ(void *handle);
static unsigned DoAIRQ(void *handle);

/* initialisation **************************************************/

BEGIN_STATE_DECL(Mycopro)

	/* store details of peripheral registration */
	ARMul_CoprocessorV5 cpv5;

END_STATE_DECL(Mycopro)

BEGIN_INIT(Mycopro)
Hostif_PrettyPrint(state->hostif, config, ", Mycopro");
{
	/* register the co-pro functions */
	ARMul_CoprocessorV5 *cp = &state->cpv5;
	cp->LDC=LDC;
	cp->STC=STC;
	cp->MRC=MRC;
	cp->MCR=MCR;
	cp->CDP=CDP;
	
	/* prototype: unsigned ARMulif_InstallCoprocessorV5(RDI_ModuleDesc *mdesc, unsigned number,
                                      struct ARMul_CoprocessorV5 *cpv5,
                                      void *data);
    */
    
    ARMulif_InstallCoprocessorV5( &state->coredesc, 4, cp, state );
}
END_INIT(Mycopro)

BEGIN_EXIT(Mycopro)
END_EXIT(Mycopro)

/* COPRO instructions *********************************************/

/* copro LDC (ARM -> Copro instruction) */

static int LDC(void *handle, int type, ARMword instr, uint32 *data)
{
	static unsigned words;
	IGNORE(handle);

	if(type != ARMul_CP_DATA) {
		words = 0;
		return(ARMul_CP_DONE);
	}

	if( BIT(22) ) { /* long access so get two words */
		ValReg[BITS(12,15)] = *data;
		if( words++ == 4 )
			return(ARMul_CP_DONE);
		else
			return(ARMul_CP_INC);
	} else { /* just get 1 word */
		ValReg[BITS(12,15)] = *data;

		return(ARMul_CP_DONE);
	}
}

/* Copro. STC instruction i.e. copro -> ARM transfer */
static int STC(void *handle, int type, ARMword instr, uint32 *data)
{
	static unsigned words;

	IGNORE(handle);

	if( type != ARMul_CP_DATA ) {
		words = 0;
		return(ARMul_CP_DONE);
	}
	if( BIT(22) ) { /* two word access */
		*data = ValReg[BITS(12,15)];
		if( words++ == 4 )
			return (ARMul_CP_DONE);
		else
			return (ARMul_CP_INC);
	}
	else { /* get 1 word */
		*data = ValReg[BITS(12,15)];
		return (ARMul_CP_DONE);
	}
}

/* copro MRC instruction. copro -> ARM transfer */
static int MRC(void *handle, int type, ARMword instr, uint32 *data)
{
	IGNORE(handle);
	IGNORE(type);
	*data = ValReg[BITS(16,19)];
	return(ARMul_CP_DONE);
}

/* copro MCR instruction.  ARM to copro transfer. */
static int MCR(void *handle, int type, ARMword instr, uint32 *data)
{
	MycoproState *state = (MycoproState *)handle;

	IGNORE(handle);
	IGNORE(type);
	ValReg[BITS(16,19)] = *data;

	return (ARMul_CP_DONE);
}

/* copro CDP instruction.  ARM causes this to be executed on copro. */
static int CDP(void *handle, int type, ARMword instr, uint32 *data)
{
	static ARMTime finish;
	ARMword howlong;
	MycoproState *state=(MycoproState *)handle;
	ARMTime Now, nextEventTime;

	howlong = ValReg[BITS(0,3)];

	switch((int)BITS(20,23)) {
	case 0: if(type == ARMul_CP_FIRST) {
		/* 1st cycle of busy wait */
		finish = ARMulif_Time(&state->coredesc) + howlong;
		if( howlong == 0 )
			return ARMul_CP_DONE;
		else
			return ARMul_CP_BUSY;
		}

		return ARMul_CP_DONE;

	case 1: if( howlong == 0 )
				ARMulif_SetSignal( &(state->coredesc), RDIPropID_ARMSignal_FIQ, FALSE );
		else {
			Now = ARMulif_Time(&state->coredesc);
			nextEventTime = Now + howlong;
  	
			ARMulif_ScheduleNewTimedCallback( &state->coredesc, DoAFIQ, state, nextEventTime, 0 );
		}
		return ARMul_CP_DONE;

	case 2: if( howlong == 0 )
				ARMulif_SetSignal( &(state->coredesc), RDIPropID_ARMSignal_IRQ, FALSE );
		else {
			Now = ARMulif_Time(&state->coredesc);
			nextEventTime = Now + howlong;
  	
			ARMulif_ScheduleNewTimedCallback( &state->coredesc, DoAIRQ, state, nextEventTime, 0 );
		}
		return ARMul_CP_DONE;

	case 3: ARMulif_SetSignal( &(state->coredesc), RDIPropID_ARMSignal_FIQ, FALSE );
		return ARMul_CP_DONE;
	case 4: ARMulif_SetSignal( &(state->coredesc), RDIPropID_ARMSignal_IRQ, FALSE );
		return ARMul_CP_DONE;
	case 5: ValReg[BITS(0,3)] = (unsigned long)ARMulif_Time(&state->coredesc);
		return ARMul_CP_DONE;
	}

	return ARMul_CP_CANT;
}

/* cause an FIQ */
static unsigned DoAFIQ(void *handle)
{
	MycoproState *state=(MycoproState *)handle;
	ARMulif_SetSignal( &(state->coredesc), RDIPropID_ARMSignal_FIQ, TRUE );
	return 0;
}

/* cause an IRQ */
static unsigned DoAIRQ(void *handle)
{
	MycoproState *state=(MycoproState *)handle;
	ARMulif_SetSignal( &(state->coredesc), RDIPropID_ARMSignal_IRQ, TRUE );
	return 0;
}

/*--- <SORDI STUFF> ---*/


#define SORDI_DLL_DESCRIPTION_STRING "Example Coprocessor model"
#define SORDI_RDI_PROCVEC Mycopro_AgentRDI
#include "perip_sordi.h"

#include "perip_rdi_agent.h"
    IMPLEMENT_AGENT_PROCS_NOEXE_NOMODULE(Mycopro)
    IMPLEMENT_AGENT_PROCVEC_NOEXE(Mycopro)

/*--- </> ---*/