rtl-stub.c

fsmlabs的real time linux的内核

	if (!f) {
		spin_unlock (&bp_lock);
		return EINVAL;
	}

	spin_unlock (&bp_lock);
	return 0;
}

int remove_bp (char *mem)
{
	struct bp_cache_entry *e = cache_start;
	struct bp_cache_entry *f = 0;
	char buf[10];
	if (!e) {
		return EINVAL;
	}
	if (e->mem == mem) {
		cache_start = e->next;
		f = e;
	} else {
		for (; e->next; e = e->next) {
			if (e->next->mem == mem) {
				f = e->next;
				e->next = f->next;
				break;
			}
		}
	}
	if (!f) {
		return EINVAL;
	}

	sprintf(buf, "%08x", (unsigned) f->val);
	hex2mem(buf, f->mem, 4);
	debugpr("removed %x, restored %s (real) %x\n", f->mem, buf, *((unsigned long *) (f->mem)));
	
	kgdb_flush_cache_all();
	flush_icache_range((unsigned) f->mem, (unsigned)( f->mem)+4);
	return 0;
}

static int send_exception_info = 0;
struct module *mod;
static void kgdb_fault_handler(struct pt_regs *regs)
{
	kgdb_longjmp((long*)fault_jmp_buf, 1);
}


/* Convert the SPARC hardware trap type code to a unix signal number. */
/*
 * This table contains the mapping between PowerPC hardware trap types, and
 * signals, which are primarily what GDB understands.
 */
static struct hard_trap_info
{
	unsigned int tt;		/* Trap type code for powerpc */
	unsigned char signo;		/* Signal that we map this trap into */
} hard_trap_info[] = {
	{ 0x200, SIGSEGV },			/* machine check */
	{ 0x300, SIGSEGV },			/* address error (store) */
	{ 0x400, SIGBUS },			/* instruction bus error */
	{ 0x500, SIGINT },			/* interrupt */
	{ 0x600, SIGBUS },			/* alingment */
	{ 0x700, SIGTRAP },			/* breakpoint trap */
	{ 0x800, SIGFPE },			/* fpu unavail */
	{ 0x900, SIGALRM },			/* decrementer */
	{ 0xa00, SIGILL },			/* reserved */
	{ 0xb00, SIGILL },			/* reserved */
	{ 0xc00, SIGCHLD },			/* syscall */
	{ 0xd00, SIGTRAP },			/* single-step/watch */
	{ 0xe00, SIGFPE },			/* fp assist */
	{ 0, 0}				/* Must be last */
};

static int computeSignal(unsigned int tt)
{
	struct hard_trap_info *ht;

	for (ht = hard_trap_info; ht->tt && ht->signo; ht++)
		if (ht->tt == tt)
			return ht->signo;

	return SIGHUP;         /* default for things we don't know about */
}

#define PC_REGNUM 64
#define SP_REGNUM 1

  
/*
 * This function does all command processing for interfacing to gdb.
 */
static int
handle_exception (struct pt_regs *regs)
{
	int signo;
	int addr;
	int length;
	char *ptr;
/* 	unsigned long nip = regs->nip; */
	unsigned int msr;
	int exceptionVector = regs->trap;

  unsigned long flags;
#ifdef CONFIG_RTL_DEBUGGER_THREADS
  pthread_t current_thread = pthread_self();
#endif

  if (user_mode(regs) && !(rtl_is_psc_active())) {
    return(0);
  }
  	if (debugger_fault_handler) {
		debugger_fault_handler(regs);
	}

	if (debugger_fault_handler) {
		debugger_fault_handler(regs);
		panic("kgdb longjump failed!\n");
	}

	rtl_hard_savef_and_cli(flags);

#ifdef KGDB_DEBUG
	printk("kgdb: entering handle_exception; trap [0x%x]\n",
	       (unsigned int)regs->trap);
#endif

	msr = get_msr();
	signo = computeSignal(exceptionVector);

	if (rtl_running_linux() && exceptionVector != 0x700  && exceptionVector != 0xd00) {
		rtl_hard_restore_flags(flags) ;
		return 0; /* let linux handle it's own faults */
	}

	/*   debugpr("exception: %x\n", exceptionVector); */
	debugpr("exception: %x regs->nip=%x regs->msr=%x msr=%x MSR_EE=%x\n",
			exceptionVector, regs->nip, regs->msr, msr, MSR_EE);

	/* can't trace with interrupts disabled or in Linux mode */
	if (exceptionVector == 0xd00 && (!(regs->msr & MSR_EE) || rtl_running_linux())) {
		regs->msr &= ~MSR_SE; /* clear trace bit */
		debugpr("can't single step");
		kgdb_flush_cache_all();
		set_msr(msr);
		rtl_hard_restore_flags(flags) ;
		return(1) ;
	}

	if (exceptionVector == 0x700) {
		/* disable breakpoints if they're hit with interrupts disabled or in Linux mode */
		if ((!(regs->msr & MSR_EE) || rtl_running_linux())) {
			spin_lock (&bp_lock);
			if (remove_bp (((char *) regs->nip))) {
				debugpr("advancing");
				regs->nip += 4; /* there's no cached breakpoint, so nothing to retry */
			}
			spin_unlock (&bp_lock);
			debugpr("can't breakpoint");
			regs->msr |= MSR_SE;
			kgdb_flush_cache_all();
			set_msr(msr);
			rtl_hard_restore_flags(flags) ;
			return(1) ;
		}
		if (*((unsigned long *)regs->nip) == BPCODE && search_bp((char *)regs->nip)) {
			/* Skip over breakpoint trap insn */
			regs->nip += 4;
		}
	}


  /* ok, here we know pthread_self() is an RT-thread */
  pthread_cleanup_push (&rtl_exit_debugger, 0);
  rtl_enter_debugger(exceptionVector, (void *) regs->nip);

  /* reply to host that an exception has occurred */
  set_bit(0, &send_exception_info);

	while (1) {
	    remcomOutBuffer[0] = 0;
	    if (test_and_clear_bit(0, &send_exception_info)) {
		strcpy(remcomInBuffer, "?");
	    } else {
		    getpacket(remcomInBuffer);
	    }

	    switch (remcomInBuffer[0]) {
		case 'q' : 
			if (!strcmp(remcomInBuffer, "qOffsets") && text && data && bss && !rtl_is_psc_active()) {
				sprintf(remcomOutBuffer, "Text=%x;Data=%x;Bss=%x",
				(unsigned )text, (unsigned )data, (unsigned )bss);
			}
#ifdef CONFIG_RTL_DEBUGGER_THREADS
	 if (!strcmp(remcomInBuffer, "qC")) {
		 sprintf(remcomOutBuffer, "QC%x", (unsigned )(pthread_self()));
	 } else if (!strncmp(remcomInBuffer, "qL", 2)) {
		 /* we assume we have a limit of 31 threads -- to fit in one packet */
		 char packethead[17];
		 pthread_t task;
		 int ntasks = 0;
		 int i;

		 strcpy(remcomOutBuffer, remcomInBuffer);

		for (i = 0; i < rtl_num_cpus(); i++) {
			 int cpu_id = cpu_logical_map (i);
			 schedule_t *s = &rtl_sched [cpu_id];

			 spin_lock (&s->rtl_tasks_lock);
			 task = s->rtl_tasks;
			 while (task != &s->rtl_linux_task && ntasks < 31) {
				 sprintf((remcomOutBuffer) + strlen(remcomOutBuffer),
						 "00000000%08x", (unsigned) task);
				 task = task->next;
				 ntasks++;
			 }
			 spin_unlock (&s->rtl_tasks_lock);
		}
		sprintf(packethead, "qM%02x%01x", ntasks, 1 /* done */);
		memcpy(remcomOutBuffer, packethead, strlen(packethead));
	 }
#endif
	 break;
#ifdef CONFIG_RTL_DEBUGGER_THREADS
      case 'H': if (/* remcomInBuffer[1] == 'c' ||*/ remcomInBuffer[1] == 'g') {
		       if (remcomInBuffer[2] == '-') {
			current_thread = (pthread_t) -strtoul(remcomInBuffer + 3, 0, 16);
		       } else  {
			current_thread = (pthread_t) strtoul(remcomInBuffer + 2, 0, 16);
		       }
			strcpy(remcomOutBuffer, "OK");
		}
		break;

      case 'T':{
		       pthread_t thread;
		       if (remcomInBuffer[1] == '-') {
			thread = (pthread_t) -strtoul(remcomInBuffer + 2, 0, 16);
		       } else  {
			thread = (pthread_t) strtoul(remcomInBuffer + 1, 0, 16);
		       }
		       if (!pthread_kill(thread, 0)) {
			       strcpy(remcomOutBuffer, "OK");
		       } else {
			       strcpy(remcomOutBuffer, "ERROR");
		       }

		}
		break;
#endif
#ifdef CONFIG_RTL_DEBUGGER_Z_PROTOCOL
      case 'Z': case 'z' :
		{
			int type = remcomInBuffer[1] - '0';
			unsigned address = strtoul(remcomInBuffer + 3, 0, 16);
			int res;
			if (type != 0) {
				strcpy(remcomOutBuffer, "ERROR");
				break;
			}
			spin_lock (&bp_lock);
			if (remcomInBuffer[0] == 'Z') {
				res = insert_bp((char *)address);
			} else {
				remove_bp((char *)address);
				res = 0;
			}
			spin_unlock (&bp_lock);
			if (res) {
				strcpy(remcomOutBuffer, "ERROR");
			} else {
				strcpy(remcomOutBuffer, "OK");
			}

		}
		break;
#endif
      case '?' :
#ifdef CONFIG_RTL_DEBUGGER_THREADS
		sprintf(remcomOutBuffer, "T%02xthread:%x;", signo, (unsigned) pthread_self());

#else
		remcomOutBuffer[0] = 'S';
                   remcomOutBuffer[1] =  hexchars[signo >> 4];
                   remcomOutBuffer[2] =  hexchars[signo % 16];
                   remcomOutBuffer[3] = 0;
#endif
                 break;
		case 'd':
			/* toggle debug flag */
			kdebug ^= 1;
			break;

		case 'g':	/* return the value of the CPU registers.
				 * some of them are non-PowerPC names :(
				 * they are stored in gdb like:
				 * struct {
				 *     u32 gpr[32];
				 *     f64 fpr[32];
				 *     u32 pc, ps, cnd, lr; (ps=msr)
				 *     u32 cnt, xer, mq;
				 * }
				 */
		{
			int i;
			struct pt_regs *t_regs = regs;
#ifdef CONFIG_RTL_DEBUGGER_THREADS
			struct pt_regs r = *regs;
			if (current_thread != pthread_self()) {
				t_regs = &r;
/*				rtl_printf("thread %x\n", current_thread);
				rtl_printf("stack=%x\n", current_thread->stack);
				for (i=0; i < 10; i++) {
					rtl_printf("%d: %#x    ", i, *(current_thread->stack + i));
				}
				rtl_printf("\n"); */
				for(i=0; i<32; i++) {
					t_regs->gpr[i] = *(current_thread->stack + 5 + i);
/* 					rtl_printf("%d: %#x    ", i, t_regs->gpr[i]); */
				}
				t_regs->msr = *(current_thread->stack + 0);
				t_regs->nip = *(current_thread->stack + 4);
			}
#endif
			ptr = remcomOutBuffer;
			/* General Purpose Regs */
			ptr = mem2hex((char *)t_regs, ptr, 32 * 4);
			/* Floating Point Regs - FIXME */
			/*ptr = mem2hex((char *), ptr, 32 * 8);*/
			for(i=0; i<(32*8*2); i++) { /* 2chars/byte */
				ptr[i] = '0';
			}
			ptr += 32*8*2;
			/* pc, msr, cr, lr, ctr, xer, (mq is unused) */
/* 			ptr = mem2hex((char *)&nip, ptr, 4); */
			ptr = mem2hex((char *)&t_regs->nip, ptr, 4);
			ptr = mem2hex((char *)&t_regs->msr, ptr, 4);
			ptr = mem2hex((char *)&t_regs->ccr, ptr, 4);
			ptr = mem2hex((char *)&t_regs->link, ptr, 4);
			ptr = mem2hex((char *)&t_regs->ctr, ptr, 4);
			ptr = mem2hex((char *)&t_regs->xer, ptr, 4);
		}
			break;

		case 'G':   /* set the value of the CPU registers */
		{
			ptr = &remcomInBuffer[1];

			/*
			 * If the stack pointer has moved, you should pray.
			 * (cause only god can help you).
			 */

			/* General Purpose Regs */
			hex2mem(ptr, (char *)regs, 32 * 4);

			/* Floating Point Regs - FIXME?? */
			/*ptr = hex2mem(ptr, ??, 32 * 8);*/
			ptr += 32*8*2;

			/* pc, msr, cr, lr, ctr, xer, (mq is unused) */
			ptr = hex2mem(ptr, (char *)&regs->nip, 4);
			ptr = hex2mem(ptr, (char *)&regs->msr, 4);
			ptr = hex2mem(ptr, (char *)&regs->ccr, 4);
			ptr = hex2mem(ptr, (char *)&regs->link, 4);
			ptr = hex2mem(ptr, (char *)&regs->ctr, 4);
			ptr = hex2mem(ptr, (char *)&regs->xer, 4);

			strcpy(remcomOutBuffer,"OK");
		}
			break;

		case 'm':	/* mAA..AA,LLLL  Read LLLL bytes at address AA..AA */
				/* Try to read %x,%x.  */

			ptr = &remcomInBuffer[1];

			if (hexToInt(&ptr, &addr)
			    && *ptr++ == ','
			    && hexToInt(&ptr, &length))	{
				if (mem2hex((char *)addr, remcomOutBuffer,length))
					break;
				strcpy (remcomOutBuffer, "E03");
			} else {
				strcpy(remcomOutBuffer,"E01");
			}
			break;

		case 'M': /* MAA..AA,LLLL: Write LLLL bytes at address AA.AA return OK */
			/* Try to read '%x,%x:'.  */

			ptr = &remcomInBuffer[1];

			if (hexToInt(&ptr, &addr)
			    && *ptr++ == ','
			    && hexToInt(&ptr, &length)
			    && *ptr++ == ':') {
#ifdef CONFIG_RTL_DEBUG_BP_HACK
				debugpr("addr=%x, length=%x, ptr=%s\n", addr,length, ptr);
				if (length == 4) { /* maybe breakpoint stuff */
					if (!strncmp(ptr, STR_BPCODE, strlen(STR_BPCODE))) {
						spin_lock (&bp_lock);
						insert_bp((char *)addr);
						spin_unlock (&bp_lock);
					} else {
						spin_lock (&bp_lock);
						remove_bp((char *)addr);
						spin_unlock (&bp_lock);
					}
				}
#endif

				if (hex2mem(ptr, (char *)addr, length)) {
					strcpy(remcomOutBuffer, "OK");
				} else {
					strcpy(remcomOutBuffer, "E03");
				}
				flush_icache_range(addr, addr+length);
			} else {
				strcpy(remcomOutBuffer, "E02");
			}
			break;


		case 'k':    /* kill the program, actually just continue */
		case 'c':    /* cAA..AA  Continue; address AA..AA optional */
			/* try to read optional parameter, pc unchanged if no parm */

			ptr = &remcomInBuffer[1];
			if (hexToInt(&ptr, &addr)) {
				regs->nip = addr;
			}

/* Need to flush the instruction cache here, as we may have deposited a
 * breakpoint, and the icache probably has no way of knowing that a data ref to
 * some location may have changed something that is in the instruction cache.
 */
			kgdb_flush_cache_all();
			regs->msr &= ~MSR_SE;
			set_msr(msr);
			kgdb_active = 0;
			goto cleanup;

		case 's':
			kgdb_flush_cache_all();
			regs->msr |= MSR_SE;
#if 0
			set_msr(msr | MSR_SE);
#endif
			kgdb_active = 0;
			goto cleanup;

		case 'r':		/* Reset (if user process..exit ???)*/
			panic("kgdb reset.");
			break;
		}			/* switch */
		if (remcomOutBuffer[0] && kdebug) {
			printk("remcomInBuffer: %s\n", remcomInBuffer);
			printk("remcomOutBuffer: %s\n", remcomOutBuffer);
		}
		/* reply to the request */
		putpacket(remcomOutBuffer);
	} /* while(1) */
cleanup: pthread_cleanup_pop(1);
  rtl_hard_restore_flags(flags) ;
  debugpr("cleanup");
  return(1) ;
}


/* Output string in GDB O-packet format if GDB has connected. If nothing
   output, returns 0 (caller must then handle output). */
/*
int
kgdb_output_string (const char* s, unsigned int count)
{
	char buffer[512];

        if (!kgdb_started)
            return 0;

	count = (count <= (sizeof(buffer) / 2 - 2)) 
		? count : (sizeof(buffer) / 2 - 2);

	buffer[0] = 'O';
	mem2hex (s, &buffer[1], count);
	putpacket(buffer);

        return 1;
 }
 */

#ifndef CONFIG_8xx

/* I don't know why other platforms don't need this.  The function for
 * the 8xx is found in arch/ppc/8xx_io/uart.c.  -- Dan
 */
void
kgdb_map_scc(void)
{
}
#endif