rtl-stub.c

rtlinux-3.2-pre3.tar.bz2 rtlinux3.2-pre3的源代码

 * For kernel version 2.1.xx the cli() actually gets a spin lock so
 * it is always necessary to do a restore_flags before returning
 * so as to let go of that lock.
 */

/* more RTLinux support */
static spinlock_t bp_lock = SPIN_LOCK_UNLOCKED;

#define RTL_MAX_BP 1024
static struct bp_cache_entry {
	char *mem;
	unsigned char val;
	struct bp_cache_entry *next;
} bp_cache [RTL_MAX_BP];

static struct bp_cache_entry *cache_start = 0;

int insert_bp(char *mem)
{
	int i;
	struct bp_cache_entry *e;
	int old;
	char buf[3];

	if (!mem2hex(mem, buf, 1)) {
		return EINVAL; /* memory error */
	}

	old = strtoul(buf, 0, 16);

	for (e = cache_start; e; e = e->next) {
		if (e->mem == mem) {
			return EINVAL; /* already there */
		}
	}
	for (i = 0; i < RTL_MAX_BP; i++) {
		if (bp_cache[i].mem == 0) {
			break;
		}
	}
	if (i == RTL_MAX_BP) {
		return EINVAL; /* no space */
	}
	bp_cache[i] . val = old;
	bp_cache[i] . mem = mem;
	bp_cache[i] . next = cache_start;
	cache_start = &bp_cache[i];

	set_char (mem, 0xcc);
	return 0;
}


int remove_bp (char *mem)
{
	struct bp_cache_entry *e = cache_start;
	struct bp_cache_entry *f = 0;
	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;
	}

	mem_err_expected = 1;
	set_char (f->mem, f->val);
  	if (mem_err) {
		return EINVAL;
	}
	mem_err_expected = 0;
	return 0;
}

#define CONFIG_RTL_DEBUGGER_THREADS
#define CONFIG_RTL_DEBUGGER_Z_PROTOCOL

extern int rtl_send_exception_info;

int handle_exception(int exceptionVector,
		     int signo,
		     int err_code,
		     struct pt_regs *linux_regs)
{
  int    addr, length;
  char * ptr;
  int    newPC;
  unsigned long flags;
  int 			gdb_regs[NUMREGBYTES/4];
#define	regs	(*linux_regs)

#ifdef CONFIG_RTL_DEBUGGER_THREADS
  pthread_t current_thread = pthread_self();
#endif
  /*
   * If the entry is not from the kernel then return to the Linux
   * trap handler and let it process the interrupt normally.
   */
  if (user_mode(linux_regs) && !(rtl_is_psc_active())) {
    return(0);
  }

  rtl_hard_savef_and_cli(flags); /* shall we get a spinlock? */

  if (remote_debug)
  {
    unsigned long	*lp = (unsigned long *) &linux_regs ;

    printk("handle_exception(exceptionVector=%d, "
           "signo=%d, err_code=%d, linux_regs=%p)\n",
	    exceptionVector, signo, err_code, linux_regs) ;
    print_regs(&regs) ;
    printk("Stk: %8lx %8lx %8lx %8lx  %8lx %8lx %8lx %8lx\n",
    	   lp[0],lp[1],lp[2],lp[3],lp[4],lp[5],lp[6],lp[7]);
    printk("     %8lx %8lx %8lx %8lx  %8lx %8lx %8lx %8lx\n",
    	   lp[8],lp[9],lp[10],lp[11],lp[12],lp[13],lp[14],lp[15]);
    printk("     %8lx %8lx %8lx %8lx  %8lx %8lx %8lx %8lx\n",
    	   lp[16],lp[17],lp[18],lp[19],lp[20],lp[21],lp[22],lp[23]);
    printk("     %8lx %8lx %8lx %8lx  %8lx %8lx %8lx %8lx\n",
    	   lp[24],lp[25],lp[26],lp[27],lp[28],lp[29],lp[30],lp[31]);
  }

  switch (exceptionVector)
  {
  case 0:			/* divide error */
  case 1:			/* debug exception */
  case 2:			/* NMI */
  case 3:			/* breakpoint */
  case 4:			/* overflow */
  case 5:			/* bounds check */
  case 6:			/* invalid opcode */
  case 7:			/* device not available */
  case 8:			/* double fault (errcode) */
  case 10:			/* invalid TSS (errcode) */
  case 12:			/* stack fault (errcode) */
  case 16:			/* floating point error */
  case 17:			/* alignment check (errcode) */
  default:			/* any undocumented */
      break ;
  case 11:			/* segment not present (errcode) */
  case 13:			/* general protection (errcode) */
  case 14:			/* page fault (special errcode) */
      if (mem_err_expected)
      {
	  /*
	   * This fault occured because of the get_char or set_char
	   * routines.  These two routines use either eax of edx to
	   * indirectly reference the location in memory that they
	   * are working with.  For a page fault, when we return
	   * the instruction will be retried, so we have to make
	   * sure that these registers point to valid memory.
	   */
	  mem_err = 1 ;		/* set mem error flag */
	  mem_err_expected = 0 ;
	  regs.eax = (long) &garbage_loc ;	/* make valid address */
	  regs.edx = (long) &garbage_loc ;	/* make valid address */
	  if (remote_debug) printk("Return after memory error\n");
	  if (remote_debug) print_regs(&regs) ;
	  rtl_hard_restore_flags(flags) ;
	  return(1) ;
      }
      break ;
  }

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

  debugpr("exception: %x\n", exceptionVector);

  /* can't trace with interrupts disabled or in Linux mode */
  if (exceptionVector == 1 && (!(regs.eflags & 0x200) || rtl_running_linux())) {
          regs.eflags &= 0xfffffeff; /* clear trace bit */
	  rtl_hard_restore_flags(flags) ;
	  return(1) ;
  }

  /* disable breakpoints if they're hit with interrupts disabled or in Linux mode */
  if (exceptionVector == 3 && (!(regs.eflags & 0x200) || rtl_running_linux())) {
	  spin_lock (&bp_lock);
	  if (remove_bp (((char *) regs.eip) - 1) == 0) {
		  --regs.eip;
	  }
	  spin_unlock (&bp_lock);
	  rtl_hard_restore_flags(flags) ;
	  return(1) ;
  }

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

  debugpr("starting talk");
  gdb_i386vector  = exceptionVector;
  gdb_i386errcode = err_code ;

  while (1==1) {
    error = 0;
    remcomOutBuffer[0] = 0;
    if (test_and_clear_bit(0, &rtl_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);
		       }
			debugpr("Hc/g: %x", (unsigned) current_thread);
			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' : remote_debug = !(remote_debug);  /* toggle debug flag */
		 printk("Remote debug %s\n", remote_debug ? "on" : "off");
                 break;
      case 'g' : /* return the value of the CPU registers */
		regs_to_gdb_regs(gdb_regs, &regs) ;
#ifdef CONFIG_RTL_DEBUGGER_THREADS
		/* we cheat here; we only provide correct eip and esp */
		if (current_thread != pthread_self()) {
			gdb_regs[_ESP] = (int) current_thread->stack;
			gdb_regs[_EBP] = *(current_thread->stack + 6);
			gdb_regs[_PC] = *(current_thread->stack);
			debugpr("reg read for %x", (unsigned) current_thread);

		}
#endif
                mem2hex((char*) gdb_regs, remcomOutBuffer, NUMREGBYTES);
                break;
      case 'G' : /* set the value of the CPU registers - return OK */
                hex2mem(&remcomInBuffer[1], (char*) gdb_regs, NUMREGBYTES);
		gdb_regs_to_regs(gdb_regs, &regs) ;
                strcpy(remcomOutBuffer,"OK");
                break;

      /* mAA..AA,LLLL  Read LLLL bytes at address AA..AA */
      case 'm' :
		    /* TRY TO READ %x,%x.  IF SUCCEED, SET PTR = 0 */
                    ptr = &remcomInBuffer[1];
                    if (hexToInt(&ptr,&addr))
                        if (*(ptr++) == ',')
                            if (hexToInt(&ptr,&length))
                            {
                                ptr = 0;
                                if (!mem2hex((char*) addr, remcomOutBuffer, length)) {
				    strcpy (remcomOutBuffer, "E03");
				    debug_error ("memory fault\n", NULL);
				}
                            }

                    if (ptr)
                    {
		      strcpy(remcomOutBuffer,"E01");
		      debug_error("malformed read memory command: %s\n",remcomInBuffer);
		    }
	          break;

      /* MAA..AA,LLLL: Write LLLL bytes at address AA.AA return OK */
      case 'M' :
		    /* TRY TO READ '%x,%x:'.  IF SUCCEED, SET PTR = 0 */
                    ptr = &remcomInBuffer[1];
                    if (hexToInt(&ptr,&addr))
                        if (*(ptr++) == ',')
                            if (hexToInt(&ptr,&length))
                                if (*(ptr++) == ':')
                                {
                                    if (!hex2mem(ptr, (char*) addr, length)) {
					strcpy (remcomOutBuffer, "E03");
					debug_error ("memory fault\n", NULL);
				    } else {
				        strcpy(remcomOutBuffer,"OK");
				    }

                                    ptr = 0;
                                }
                    if (ptr)
                    {
		      strcpy(remcomOutBuffer,"E02");
		      debug_error("malformed write memory command: %s\n",remcomInBuffer);
		    }
                break;

     /* cAA..AA    Continue at address AA..AA(optional) */
     /* sAA..AA   Step one instruction from AA..AA(optional) */
     case 'c' :
     case 's' :
          /* try to read optional parameter, pc unchanged if no parm */
         ptr = &remcomInBuffer[1];
         if (hexToInt(&ptr,&addr))
	 {
	     if (remote_debug)
		printk("Changing EIP to 0x%x\n", addr) ;

             regs.eip = addr;
	 }

          newPC = regs.eip ;

          /* clear the trace bit */
          regs.eflags &= 0xfffffeff;

          /* set the trace bit if we're stepping */
          if (remcomInBuffer[0] == 's') regs.eflags |= 0x100;

	  if (remote_debug)
	  {
	    printk("Resuming execution\n") ;
	    print_regs(&regs) ;
	  }
	  debugpr("cont\n");
	  goto cleanup;

      /* kill the program */
      case 'k' :
/* 	  last_module = 0; */
	  goto cleanup;
/*      default:
	  rtl_debug_handler(); */
      } /* switch */

    /* reply to the request */
    putpacket(remcomOutBuffer);
  }

cleanup: pthread_cleanup_pop(1);
  rtl_hard_restore_flags(flags) ;
  return(1) ;
#undef regs
}

static struct hard_trap_info
{
	unsigned int tt;		/* Trap type code for i386 */
	unsigned char signo;		/* Signal that we map this trap into */
} hard_trap_info[] = {
	{ 0,  SIGFPE },
	{ 1, SIGTRAP },
	{ 2, SIGSEGV },
	{ 3, SIGTRAP },
	{ 4, SIGSEGV },
	{ 5, SIGSEGV },
	{ 6, SIGILL },
	{ 7, SIGSEGV },
	{ 8, SIGSEGV },
	{ 9, SIGFPE },
	{ 10, SIGSEGV },
	{ 11, SIGBUS },
	{ 12, SIGBUS },
	{ 13, SIGSEGV },
	{ 17, SIGSEGV },
	{ 18, SIGSEGV },
	{ 19, SIGSEGV }
};

static int computeSignal(unsigned int tt)
{
	int i;

	for (i = 0; i < sizeof(hard_trap_info)/sizeof(hard_trap_info[0]); i++) {
		if (hard_trap_info[i].tt == tt) {
			return hard_trap_info[i].signo;
		}
	}

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

int rtl_debug_exception(int vector, struct pt_regs *regs, int error_code)
{
	int signo = computeSignal(vector);
/*	if (rtl_running_linux()) {
		rtl_cprintf("linux ");
	} else {
		rtl_cprintf("rt_thread ");
	}
	rtl_cprintf("vector=%d, eflags=%x, xcs=%x\n", vector, regs->eflags , regs->xcs);
	*/
	return handle_exception(vector, signo, error_code, regs);
}