nios_gdb_stub.c

ALTERA的NIOS处理器！文件直接可以打开直接选择器件重新编译！

	// MA,L:values -- write to memory

	w = HexToMem(w,(char *)startAddr,byteCount);

	// Send "OK"
	PutGDBOKPacket(aBuffer);
	}

int Debug_Read_Intercept (char *aBuffer)
{
	int cnt=0;
	unsigned int data;
	int index;
	unsigned char *w;

	w = aBuffer;
	w++;	//skip past the g
	if (*w++ == 'g')  //see if this is a special "register read" packet
	{
	    w = Hex2Value(w,&index); //get the index of the register to be read

	    nm_debug_get_reg (data, index);
	    
	    //assemble the output packet
	    w=aBuffer;	//reset w to beginning of buffer
	    w = MemToHex((char *)&data, w, sizeof (data));
	    *w++ = 0;

	    //now send it
	    PutTracePacket (aBuffer,sizeof (data));

	    return 1;
	}
	return 0;
}

// Return the values of all the registers
void DoGDBCommand_g(NiosGDBGlobals *g)
	{
	char *w;

	if (Debug_Read_Intercept (g->textBuffer)) return;

	w = g->textBuffer;

	w = MemToHex((char *)(&g->registers),w,sizeof(g->registers));
	PutGDBPacket(g->textBuffer);
	GDB_Print2("Sent            Registers",0,0);
	}

int Debug_Write_Intercept (char *aBuffer)
{
	int cnt=0;
	unsigned int data;
	unsigned char code;
	int index;
	unsigned char *w;
	unsigned short dataAccumulate;
	int status;

	w = aBuffer;
	w++;	//skip past the g
	if (*w++ == 'g')  //see if this is a special "register read" packet
	{
	    w = Hex2Value(w,&index); //get the index of the register to be written
	    w++;				// past ','
	    w = Hex2Value(w,&data);

	    nm_debug_set_reg (data, index);
	    
	    //now send it
	    // Send "OK"
	    PutGDBOKPacket(aBuffer);

	    return 1;
	}
	return 0;
}

void DoGDBCommand_G(NiosGDBGlobals *g)
	{
	char *w;

	if (Debug_Write_Intercept (g->textBuffer)) return;

	w = g->textBuffer;
	w++;	// skip past 'G'
	w = HexToMem(w,(char *)(&g->registers), sizeof(g->registers) ); 

	// Send "OK"
	PutGDBOKPacket(g->textBuffer);

	GDB_Print2("Received        Registers",0,0);
    }
	

// Return last signal value
void DoGDBCommand_qm(NiosGDBGlobals *g)
	{
	char *w;

	w = g->textBuffer;

	*w++ = 'S';
	*w++ = '2';
	*w++ = '3';	// make up a signal for now...
	*w++ = 0;
	PutGDBPacket(g->textBuffer);
	}


void GDBInsertBreakpoint(NiosGDBGlobals *g,short *address)
	{
	NiosGDBBreakpoint *b;

	GDB_Print2("breakpoint 0x%x",(int)address,0);
	if(g->breakpointCount < kMaximumBreakpoints)
		{
		b = &g->breakpoint[g->breakpointCount++];
		b->address = address;
		b->oldContents = *b->address;
		*b->address = 0x7904;
		}
	}

void GDBRemoveBreakpoints(NiosGDBGlobals *g)
	{
	NiosGDBBreakpoint *b;
	int i;

	for(i = 0; i < g->breakpointCount; i++)
		{
		b = &g->breakpoint[i];
		*b->address = b->oldContents;
		b->address = 0;
		}

	g->breakpointCount = 0;

#ifdef ETHER_DEBUG
#ifdef ethernet_exists
    // lets also clear the plugs interrupt break if there is one
    if (g->interruptee_pc)
        {
        *(g->interruptee_pc) = g->interruptee_instr;
        g->interruptee_pc = 0;
        g->interruptee_instr = 0;
        }
#endif
#endif

	}

int NiosInstructionIsTrap5(unsigned short instruction)
	{
	return instruction == 0x7905;
	}

int NiosInstructionIsPrefix(unsigned short instruction)
	{
	return (instruction >> 11) == 0x13;
	}

int NiosInstructionIsSkip(unsigned short instruction)
	{
	int op6;
	int op11;

	op6 = (instruction >> 10);
	op11 = (instruction >> 5);

	return (op6 == 0x14		// SKP0
		|| op6 == 0x15		// SKP1
		|| op11 == 0x3f6	// SKPRz
		|| op11 == 0x3f7	// SKPS
		|| op11 == 0x3fa);	// SKPRnz
	}

int NiosInstructionIsBranch(unsigned short instruction,short *pc,short **branchTargetOut)
	{
	int op4;
	int op7;
	int op10;
	short *branchTarget = 0;
	int result = 0;

	op4 = (instruction >> 12);
	op7 = (instruction >> 9);
	op10 = (instruction >> 6);

	if(op4 == 0x08)		// BR, BSR
		{
		int offset;

		result = 1;
		offset = instruction & 0x07ff;
		if(offset & 0x400)	// sign extend
			offset |= 0xffffF800;
		branchTarget = pc + offset + 1;	// short * gets x2 scaling automatically
		}
	else if(op10 == 0x1ff)	// JMP, CALL
		{
		result = 1;
		branchTarget = (short *)(gdb.registers.r[instruction & 31] * 2);
		}
	else if(op7 == 0x3d)	// JMPC, CALLC
		{
		result = 1;
		branchTarget = pc + 1 + (instruction & 0x0ffff);
#ifdef __nios32__
		branchTarget = (short *)((int)branchTarget & 0xffffFFFc);	// align 32...
#else
		branchTarget = (short *)((int)branchTarget & 0xFFFe);		// align 16...
#endif
		branchTarget = (short *)(*(int *)branchTarget);
		}

	if(branchTargetOut)
		*branchTargetOut = branchTarget;

	return result;
	}

// -------------------------
// Step at address
//
// "stepping" involves inserting a
// breakpoint at some reasonable
// spot later than the current program
// counter
//
// On the Nios processor, this is
// nontrivial. For example, we should
// not break up a PFX instruction.

void DoGDBCommand_s(NiosGDBGlobals *g)
	{
	char *w;
	int x;
	short *pc;
	short *branchTarget;
	unsigned short instruction;
	int stepType;

	/*
	 * First, if there's an argument to the packet,
	 * set the new program-counter value
	 */

	w = g->textBuffer;
	w++;
	if(HexCharToValue(*w) >= 0)
		{
		w = Hex2Value(w,&x);
		g->registers.pc = x;
		}

	/*
	 * Scan forward to see what the
	 * most appropriate location(s) for
	 * a breakpoint will be.
	 *
	 * The rules are:
	 *  1. If *pc == PFX, break after modified instruction.
	 *  2. If *pc == BR,BSR,JMP,CALL, break at destination
	 *  3. If *pc == SKIP, break right after SKIP AND after optional instruction,
	                 which might, of course, be prefixed.
	 *  4. Anything else, just drop in the breakpoint.
	 */

	pc = (short *)(int)g->registers.pc;

	instruction = *pc;
	stepType = 0;

	if(NiosInstructionIsPrefix(instruction))
		{
		/*
		 * PFX instruction: skip til after it
		 */
		while(NiosInstructionIsPrefix(instruction))
			{
			pc++;
			instruction = *pc;
			}

		GDBInsertBreakpoint(g,pc + 1);
		stepType = 1;
		}
	else if(NiosInstructionIsBranch(instruction,pc,&branchTarget))
		{
		GDBInsertBreakpoint(g,branchTarget);
		stepType = 2;
		}
	else if(NiosInstructionIsSkip(instruction))
		{
		short *pc2;
		stepType = 3;

		/*
		 * Skip gets to breaks: one after the skippable instruction,
		 * and the skippable instruction itself.
		 *
		 * Since Skips know how to skip over PFX's, we have to, too.
		 */
		pc2 = pc;	// the Skip instruction
		do
			{
			pc2++;
			} while(NiosInstructionIsPrefix(*pc2));
		// pc2 now points to first non-PFX after Skip
		GDBInsertBreakpoint(g,pc2+1);
		GDBInsertBreakpoint(g,pc+1);
		}
	else
		GDBInsertBreakpoint(g,pc+1);		// the genericest case

	GDB_Print2("Program Steppingat 0x%x (%d)",g->registers.pc,stepType);
	}

// -----------------------------
// Continue at address

void DoGDBCommand_c(NiosGDBGlobals *g)
	{
	char *w;
	int x;
	w = g->textBuffer;

	w++;		// past command

	// Anything in the packet? if so,
	// use it to set the PC value

	if(HexCharToValue(*w) >= 0)
		{
		w = Hex2Value(w,&x);
		g->registers.pc = x;
		}

	GDB_Print2("Program Running at 0x%x",g->registers.pc,0);
	}

// ----------------------
// Kill

void DoGDBCommand_k(NiosGDBGlobals *g)
	{
	return;
	}


/*
 * If we've somehow skidded
 * to a stop just after a PFX instruction
 * back up the program counter by one.
 *
 * That way, we can't end up with an accidentally-unprefixed
 * instruction.
 *
 * We do this just before we begin running
 * again, so that when the host queries our
 * registers, we report the place we actually
 * stopped.
 */

void MaybeAdjustProgramCounter(NiosGDBGlobals *g)
	{
	short instruction;
	if(g->registers.pc)
		{
		instruction = *(short *)(int)(g->registers.pc - 2);
		if(NiosInstructionIsPrefix(instruction))
			g->registers.pc -= 2;
		else
			{
			// If the *current* instruction is Trap5, we must skip it!
			instruction = *(short *)(int)(g->registers.pc);
			if(NiosInstructionIsTrap5(instruction))
				g->registers.pc += 2;
			}
		}
	}

/* 
 * GDBMainLoop - this is the main processing loop
 * for the GDB stub.
 */
void GDBMainLoop ()
{
	while(1)
	{
		if (GetGDBPacket(gdb.textBuffer) > 0)
		{

			GDB_Print2(gdb.textBuffer,0,0);
			switch(gdb.textBuffer[0])
			{
			case 's':
				DoGDBCommand_s(&gdb);
				goto startRunning;
				break;

			case 'c':	// continue
				DoGDBCommand_c(&gdb);

				// if the PC is something other than 0, it's
				// probably ok to exit and go there

			startRunning:
				if(gdb.registers.pc)
				{
					MaybeAdjustProgramCounter(&gdb);
					return;
				}
				break;

			case 'm':	// memory read
				DoGDBCommand_m(gdb.textBuffer);
				break;

			case 'M':	// memory set
				DoGDBCommand_M(gdb.textBuffer);
				break;

			case 'g':	// registers read
				DoGDBCommand_g(&gdb);
				break;

			case 'G':	//registers set
				DoGDBCommand_G(&gdb);
				break;

			case 'k':	//kill process
				DoGDBCommand_k(&gdb);
				break;

			case '?':	// last exception value
				DoGDBCommand_qm(&gdb);
				break;

			default:	// return empty packet, means "yeah yeah".
				gdb.textBuffer[0] = 0;
				PutGDBPacket(gdb.textBuffer);
			break;
			}
		}
	}

}

// ----------main------------

void GDBMain(void)
{
	char c;
	int i;

	for(i = 0; i < kTextBufferSize; i++)
		gdb.textBuffer[i] = i;

	GDBRemoveBreakpoints(&gdb);

	// Send trapnumber for breakpoint encountered. No other signals.

	gdb.textBuffer[0] = 'S';
	gdb.textBuffer[1] = '0';

#if nasys_debug_core
	if (gdb.trapNumber == nasys_debug_core_irq)
	{
	    gdb.textBuffer[2] = '8';
	}
	else
	{
	    gdb.textBuffer[2] = '5';
	}
#else
	gdb.textBuffer[2] = '5';
#endif
	gdb.textBuffer[3] = 0;
	PutGDBPacket(gdb.textBuffer);

	GDB_Print2("Trap %2d         At 0x%x",
		gdb.trapNumber,gdb.registers.pc);
//	printf ("Trap %d at 0x%x\n",gdb.trapNumber,gdb.registers.pc);
//	for (i=0;i<32;i++) printf ("	register[%d] = 0x%x\n",i,gdb.registers.r[i]);

	GDBMainLoop (GDB_WHOLE_PACKET);
}

// +----------------------------------
// | gdb_eth_proc -- gets called for udp packets
// | from the host bound for gdb 
#ifdef ETHER_DEBUG
#ifdef ethernet_exists
int gdb_eth_proc(int plug_handle,
		void *context,
		ns_plugs_packet *p,
		void *payload,
		int payload_length)
{
	int i;
	char *buf = (char *)payload;

    // is this a stop request from the host??
	if (*buf == 3) 
    {
        // If plugs interrupts are enabled, we set a breakpoint at the
        // return address from the plugs ISR
        // we'll have to clear it later
        if (nr_plugs_interrupts_enabled())
        {
            gdb.interruptee_pc = (short *)nr_plugs_get_interruptee_pc ();
            if (gdb.interruptee_pc)
            {
                gdb.interruptee_instr = *gdb.interruptee_pc;
                *gdb.interruptee_pc = GDB_TRAP3;
            }
        }
	    // if plugs interrupts are not enabled, 
        // we set a flag to stop after nr_plugs_idle_exclusive
        else
        {
            gdb.stop = 1;
        }
    }

	// if we're waiting for an ack, check that here
	if (gdb.ACKstatus == ne_gdb_ack_waiting) 
	{
		if (buf[0] == '+')
		{
			gdb.ACKstatus = ne_gdb_ack_acked;
			return 0;
		}
		else if (buf[0] == '-')
		{
			gdb.ACKstatus = ne_gdb_ack_nacked;
			return 0;
		}
	}
	strcpy (gdb.textBuffer, buf);	//all commands should be zero terminated strings

	gdb.textBuffer[payload_length] = 0;	//terminate string
	
	gdb.host_ip_address=((ns_plugs_ip_packet *)(p[ne_plugs_ip].header))->source_ip_address;
	gdb.host_port_number=((ns_plugs_udp_packet *)(p[ne_plugs_udp].header))->source_port;

	return 0;
}

int nr_dbg_plugs_idle (void)
{
	int result;

	result = nr_plugs_idle ();
	if (gdb.stop)
	{
		gdb.stop = 0;
		asm ("TRAP #5");
	}
	return result;
}
#endif
#endif


/*
 * int main(void)
 *
 * All we really do here is install our trap # 3,
 * and call it once, so that we're living down in
 * the GDBMain, trap handler.
 */

extern int StubBreakpointHandler;
extern int StubHarmlessHandler;
#if nasys_debug_core
extern int StubHWBreakpointHandler;
#endif
#ifdef nasys_debug_uart
extern int StubUartHandler;
#endif

void gdb_local_install(int active)
{
	unsigned int *vectorTable;
	unsigned int stubBreakpointHandler;
	unsigned int stubHarmlessHandler;
	unsigned int stubHWBreakpointHandler;

	gdb.breakpointCount = 0;
	gdb.textBuffer[0] = 0;

	vectorTable = (int *)nasys_vector_table;
	stubBreakpointHandler = ( (unsigned int)(&StubBreakpointHandler) ) >> 1;
	stubHarmlessHandler = ( (unsigned int)(&StubHarmlessHandler) ) >> 1;
#if nasys_debug_core
	stubHWBreakpointHandler = ( (unsigned int)(&StubHWBreakpointHandler) ) >> 1;
#endif

	/*
	 * Breakpoint & single step both go here
	 */
	vectorTable[na_BreakpointTrap] = stubBreakpointHandler;
	vectorTable[na_SingleStepTrap] = stubBreakpointHandler;
	vectorTable[na_StartGDBTrap] = active ? stubBreakpointHandler : stubHarmlessHandler;
	/*
	 * If it exists, Hardware Breakpoint has a different entry point
	 */
#if nasys_debug_core
	vectorTable[na_debug_peripheral_irq] = stubHWBreakpointHandler;
#endif

#ifdef nasys_debug_uart
	if (gdb.comlink == ne_gdb_serial)
	{
		np_uart *uart = (np_uart *)nasys_debug_uart;
		unsigned int stubUartHandler = ((unsigned int)(&StubUartHandler)) >> 1;

		vectorTable[nasys_debug_uart_irq] = stubUartHandler;	  //set Uart int vector
		uart->np_uartcontrol = np_uartcontrol_irrdy_mask; //enable Rx intr
	}
#endif
}

void nios_gdb_install(int active)
{
	gdb.comlink = ne_gdb_serial;
	gdb_local_install (active);
}

#ifdef ETHER_DEBUG
#ifdef ethernet_exists
void nios_gdb_install_ethernet (int active)
{
	int result;
	host_16	host_port = GDB_ETH_PORT;

	gdb.comlink = ne_gdb_ethernet;
	gdb_local_install (active);

	result = nr_plugs_create (&gdb.gdb_eth_plug, ne_plugs_udp, host_port, gdb_eth_proc, 0, 0);
	//if unabled to open ethernet plug, switch back to default serial interface
	if (result) 
	{
		printf ("nr_plugs_create failed %d\n",result);
		gdb.comlink = ne_gdb_serial;
		return;
	}
	result = nr_plugs_connect (gdb.gdb_eth_plug, 0, -1, -1);
	if (result)
	{
		printf ("nr_plugs_connect fialed %d\n",result);
		gdb.comlink = ne_gdb_serial;
		return;
	}
}
#endif
#endif

#ifdef nios_gdb_breakpoint
	#undef nios_gdb_breakpoint
#endif

void nios_gdb_breakpoint(void)
	{
	/*
	 * If you arrived here, you didn't include
	 * the file "nios_peripherals.h", which
	 * defines nios_gdb_breakpoint as a
	 * macro that expands to TRAP 5.
	 *
	 * (No problem, you can step out
	 * of this routine.)
	 */
	asm("TRAP 5");
	}

// end of file