kgdb-stub.c

一个2.4.21版本的嵌入式linux内核

#ifdef CONFIG_KGDB_THREAD
        /* Until GDB specifies a thread */
        current_thread = NULL;
        trapped_thread = current;
#endif

	/*
	 * Check to see if this is a compiled in breakpoint
	 * (sysrq-G or initial breakpoint).  If so, we
	 * need to increment the PC to the next instruction
	 * so that we don't infinite loop.
	 *
	 * NOTE: THIS COULD BE BAD.  We're reading the PC and
	 * if the cause of the fault is a bad PC, we're going
	 * to suffer massive death. Need to find some way to
	 * validate the PC address or use our setjmp/longjmp.
	 */
	if (sigval == SIGTRAP) {
		/* Only do this check for the SIGTRAP case! */
		if (*(unsigned int *)(kgdb_regs.ARM_pc) == KGDB_COMPILED_BREAK)
			kgdb_regs.ARM_pc += 4;
	}


	undo_single_step(); /* handles cleanup upon step/stepi return */

	while (1) {
		remcomOutBuffer[0] = 0;

		kgdb_get_packet(remcomInBuffer, BUFMAX);

		switch (remcomInBuffer[0])
		{
		case '?':	/* Report most recent signal */
			send_signal_msg(sigval);
			break;

		case 'g':	/* return the values of the CPU registers */
			send_regs_msg();
               		break;

     		case 'G':	/* set the values of the CPU registers */
			set_regs_msg();
			break;

		case 'm':	/* Read LLLL bytes address AA..AA */
			read_mem_msg();
	          	break;

		case 'M':	/* Write LLLL bytes address AA.AA ret OK */
			write_mem_msg(0);
                	break;

		case 'X':       /* Write LLLL bytes esc bin address AA..AA */
			/* WARNING: UART must be configured for 8-bit chars */
			write_mem_msg(1); /* 1 = data in binary */
			break;

		case 'C':	/* Continue, signum included, we ignore it */
			continue_with_sig_msg();
			goto exit_kgdb;

		case 'c':	/* Continue at address AA..AA (optional) */
			continue_msg();
			goto exit_kgdb;

		case 'S':	/* Step, signum included, we ignore it */
			step_with_sig_msg();
			goto exit_kgdb;

		case 's':	/* Step one instruction from AA..AA */
			step_msg();
			goto exit_kgdb;

		case 'H':	/* Task related */
			set_thread_msg();
			break;

#ifdef CONFIG_KGDB_THREAD
		case 'T':	/* Query thread status */
			thread_status_msg();
			break;

		case 'q':	/* Handle query - currently thread-related */
			query_msg();
			break;
#endif

		case 'k':	/* kill the program - do nothing */
			break;

		case 'D':	/* Detach from program, send reply OK */
			kgdb_enabled = 0;
			send_ok_msg();
			kgdb_serial_getchar();
			goto exit_kgdb;

		case 'd':	/* toggle debug flag */
			remote_debug = !(remote_debug);
               		break;

		default:
			send_empty_msg();
			break;
		}
    	}

exit_kgdb:
	*trap_regs = kgdb_regs; /* Copy back any register updates */
	cpu_cache_clean_invalidate_all();
	restore_flags(flags);
}

/*
 * TODO: If remote GDB disconnects from us, we need to return
 * 0 as we're no longer active.  Does GDB send us a disconnect
 * message??
 */
int kgdb_active(void)
{
	return kgdb_enabled;
}


/* Trigger a breakpoint by function */
void breakpoint(void)
{
        if (!kgdb_enabled) {
                kgdb_enabled = 1;
        }
        BREAKPOINT();
}


/*
 * Code to determine next PC based on current PC address.
 * Taken from GDB source code.  Open Source is good. :)
 */
#define read_register(x) regs->uregs[x]

#define addr_bits_remove(x) (x & 0xfffffffc)

#define submask(x) ((1L << ((x) + 1)) - 1)
#define bit(obj,st) (((obj) >> (st)) & 1)
#define bits(obj,st,fn) (((obj) >> (st)) & submask ((fn) - (st)))
#define sbits(obj,st,fn) \
  ((long) (bits(obj,st,fn) | ((long) bit(obj,fn) * ~ submask (fn - st))))
#define BranchDest(addr,instr) \
  ((unsigned) (((long) (addr)) + 8 + (sbits (instr, 0, 23) << 2)))


/* Instruction condition field values.  */
#define INST_EQ         0x0
#define INST_NE         0x1
#define INST_CS         0x2
#define INST_CC         0x3
#define INST_MI         0x4
#define INST_PL         0x5
#define INST_VS         0x6
#define INST_VC         0x7
#define INST_HI         0x8
#define INST_LS         0x9
#define INST_GE         0xa
#define INST_LT         0xb
#define INST_GT         0xc
#define INST_LE         0xd
#define INST_AL         0xe
#define INST_NV         0xf

#define FLAG_N          0x80000000
#define FLAG_Z          0x40000000
#define FLAG_C          0x20000000
#define FLAG_V          0x10000000

#define error(x)

static unsigned long
shifted_reg_val (unsigned long inst, int carry, unsigned long pc_val,
		 unsigned long status_reg, struct pt_regs* regs)
{
  unsigned long res = 0, shift = 0;
  int rm = bits (inst, 0, 3);
  unsigned long shifttype = bits (inst, 5, 6);

  if (bit (inst, 4))
    {
      int rs = bits (inst, 8, 11);
      shift = (rs == 15 ? pc_val + 8 : read_register (rs)) & 0xFF;
    }
  else
    shift = bits (inst, 7, 11);

  res = (rm == 15
	 ? ((pc_val | (1 ? 0 : status_reg))
	    + (bit (inst, 4) ? 12 : 8)) : read_register (rm));

  switch (shifttype)
    {
    case 0:			/* LSL */
      res = shift >= 32 ? 0 : res << shift;
      break;

    case 1:			/* LSR */
      res = shift >= 32 ? 0 : res >> shift;
      break;

    case 2:			/* ASR */
      if (shift >= 32)
	shift = 31;
      res = ((res & 0x80000000L)
	     ? ~((~res) >> shift) : res >> shift);
      break;

    case 3:			/* ROR/RRX */
      shift &= 31;
      if (shift == 0)
	res = (res >> 1) | (carry ? 0x80000000L : 0);
      else
	res = (res >> shift) | (res << (32 - shift));
      break;
    }

  return res & 0xffffffff;
}

/* Return number of 1-bits in VAL.  */

static int
bitcount (unsigned long val)
{
  int nbits;
  for (nbits = 0; val != 0; nbits++)
    val &= val - 1;		/* delete rightmost 1-bit in val */
  return nbits;
}

static int
condition_true (unsigned long cond, unsigned long status_reg)
{
	if (cond == INST_AL || cond == INST_NV)
		return 1;

	switch (cond)
	{
		case INST_EQ:
		  return ((status_reg & FLAG_Z) != 0);
		case INST_NE:
		  return ((status_reg & FLAG_Z) == 0);
		case INST_CS:
		  return ((status_reg & FLAG_C) != 0);
		case INST_CC:
		  return ((status_reg & FLAG_C) == 0);
		case INST_MI:
		  return ((status_reg & FLAG_N) != 0);
		case INST_PL:
		  return ((status_reg & FLAG_N) == 0);
		case INST_VS:
		  return ((status_reg & FLAG_V) != 0);
		case INST_VC:
		  return ((status_reg & FLAG_V) == 0);
		case INST_HI:
		  return ((status_reg & (FLAG_C | FLAG_Z)) == FLAG_C);
		case INST_LS:
		  return ((status_reg & (FLAG_C | FLAG_Z)) != FLAG_C);
		case INST_GE:
		  return (((status_reg & FLAG_N) == 0) == ((status_reg & FLAG_V) == 0));
		case INST_LT:
		  return (((status_reg & FLAG_N) == 0) != ((status_reg & FLAG_V) == 0));
		case INST_GT:
		  return (((status_reg & FLAG_Z) == 0) &&
					(((status_reg & FLAG_N) == 0) == ((status_reg & FLAG_V) == 0)));
		case INST_LE:
		  return (((status_reg & FLAG_Z) != 0) ||
					(((status_reg & FLAG_N) == 0) != ((status_reg & FLAG_V) == 0)));
	}
	return 1;
}

unsigned long
get_next_pc(struct pt_regs *regs)
{
  unsigned long pc_val;
  unsigned long this_instr;
  unsigned long status;
  unsigned long nextpc;

  pc_val = regs->ARM_pc;
  this_instr = *((unsigned long *)regs->ARM_pc);
  status = regs->ARM_cpsr;
  nextpc = pc_val + 4;	/* Default case */

  if (condition_true (bits (this_instr, 28, 31), status))
  {
	  switch (bits (this_instr, 24, 27))
	  {
		case 0x0:
		case 0x1:		/* data processing */
		case 0x2:
		case 0x3:
		{
			unsigned long operand1, operand2, result = 0;
		 	unsigned long rn;
			int c;

			if (bits (this_instr, 12, 15) != 15)
				break;

			if (bits (this_instr, 22, 25) == 0
					&& bits (this_instr, 4, 7) == 9)
				/* multiply */
				error ("Illegal update to pc in instruction");

			/* Multiply into PC */
			c = (status & FLAG_C) ? 1 : 0;
			rn = bits (this_instr, 16, 19);
			operand1 = (rn == 15) ? pc_val + 8 : read_register (rn);

			if (bit (this_instr, 25))
			{
				unsigned long immval = bits (this_instr, 0, 7);
				unsigned long rotate = 2 * bits (this_instr, 8, 11);
				operand2 = ((immval >> rotate) | (immval << (32 - rotate)))
									& 0xffffffff;
			}
			else		/* operand 2 is a shifted register */
				operand2 = shifted_reg_val (this_instr, c, pc_val, status, regs);

			switch (bits (this_instr, 21, 24))
			{
				case 0x0:	/*and */
					result = operand1 & operand2;
					break;

				case 0x1:	/*eor */
					result = operand1 ^ operand2;
					break;

				case 0x2:	/*sub */
					result = operand1 - operand2;
					break;

				case 0x3:	/*rsb */
					result = operand2 - operand1;
					break;

				case 0x4:	/*add */
					result = operand1 + operand2;
					break;

				case 0x5:	/*adc */
					result = operand1 + operand2 + c;
					break;

				case 0x6:	/*sbc */
					result = operand1 - operand2 + c;
					break;

				case 0x7:	/*rsc */
					result = operand2 - operand1 + c;
					break;

				case 0x8:
				case 0x9:
				case 0xa:
				case 0xb:	/* tst, teq, cmp, cmn */
					result = (unsigned long) nextpc;
					break;

				case 0xc:	/*orr */
					result = operand1 | operand2;
					break;

				case 0xd:	/*mov */
					/* Always step into a function.  */
					result = operand2;
					break;

				case 0xe:	/*bic */
					result = operand1 & ~operand2;
					break;

				case 0xf:	/*mvn */
					result = ~operand2;
					break;
			}
			nextpc = addr_bits_remove(result);

			break;
	  }

		case 0x4:
		case 0x5:		/* data transfer */
		case 0x6:
		case 0x7:
		if (bit (this_instr, 20))
		{
			/* load */
			if (bits (this_instr, 12, 15) == 15)
			{
				/* rd == pc */
				unsigned long rn;
				unsigned long base;

				if (bit (this_instr, 22))
					error ("Illegal update to pc in instruction");

				/* byte write to PC */
				rn = bits (this_instr, 16, 19);
				base = (rn == 15) ? pc_val + 8 : read_register (rn);
				if (bit (this_instr, 24))
				{
					/* pre-indexed */
					int c = (status & FLAG_C) ? 1 : 0;
					unsigned long offset =
							  (bit (this_instr, 25)
								? shifted_reg_val (this_instr, c, pc_val, status, regs)
								: bits (this_instr, 0, 11));

					if (bit (this_instr, 23))
						base += offset;
					else
						base -= offset;
				}
				nextpc = *((unsigned long *) base);

				nextpc = addr_bits_remove (nextpc);

				if (nextpc == regs->ARM_pc)
						  error ("Infinite loop detected");
			}
		}
		break;

		case 0x8:
		case 0x9:		/* block transfer */
		if (bit (this_instr, 20))
		{
			/* LDM */
			if (bit (this_instr, 15))
			{
				/* loading pc */
				int offset = 0;

				if (bit (this_instr, 23))
				{
					/* up */
					unsigned long reglist = bits (this_instr, 0, 14);
					offset = bitcount (reglist) * 4;
					if (bit (this_instr, 24))		/* pre */
						offset += 4;
				}
				else if (bit (this_instr, 24))
					offset = - 4;

				{
					unsigned long rn_val =
						read_register (bits (this_instr, 16, 19));
					nextpc = *((unsigned int *) (rn_val + offset));
				}

				nextpc = addr_bits_remove (nextpc);
				if (nextpc == regs->ARM_pc)
						  error ("Infinite loop detected");
			}
		}
		break;

		case 0xb:		/* branch & link */
		case 0xa:		/* branch */
		{
			nextpc = BranchDest (regs->ARM_pc, this_instr);
			nextpc = addr_bits_remove (nextpc);

			if (nextpc == regs->ARM_pc)
				error ("Infinite loop detected");

			break;
		}

		case 0xc:
		case 0xd:
		case 0xe:		/* coproc ops */
		case 0xf:		/* SWI */
			break;

		default:
			error("Bad bit-field extraction");
			return (regs->ARM_pc);
	  }
  }

  return nextpc;
}