void frame_dump_stack(FILE *out, uint16 fp) {
char buf[10];
char *funcname;
int lastpc = pc;
int i;
thread_info *thread;
thread = fp == GET_REG16(7) ? current_thread : hash_get(&threads, fp);
if (!thread)
return;
for (i = thread->num_frames; i >= 0; i--) {
int fpc = thread->frames[i].pc & ~1;
int fp = thread->frames[i].fp;
funcname = symbols_get(fpc, 0);
if (!funcname) {
snprintf(buf, 10, "0x%04x", fpc);
funcname=buf;
}
fprintf(out, " %04x: %30s [%04x+%04x]\n",
fp, funcname, fpc, lastpc - fpc);
lastpc = (memory[fp] << 8 | memory[fp+1]);
}
return;
}
static int nor_read(mtd_t *_dev, void *_dest, uint32_t _off, int _amt)
{
uint16_t* alignedBuffer = (uint16_t*)_dest;
int len = _amt;
for(; len >= 2; len -= 2)
{
*alignedBuffer = GET_REG16(NOR + _off);
_off += 2;
alignedBuffer++;
}
if(len > 0) {
uint16_t lastWord = GET_REG16(NOR + _off);
uint8_t* unalignedBuffer = (uint8_t*) alignedBuffer;
*unalignedBuffer = *((uint8_t*)(&lastWord));
}
return _amt;
}
static int nor_device_init(nor_device_t *_dev)
{
nor_prepare(&_dev->mtd);
SET_REG16(NOR + NOR_COMMAND, COMMAND_UNLOCK);
SET_REG16(NOR + LOCK, LOCK_UNLOCK);
SET_REG16(NOR + NOR_COMMAND, COMMAND_IDENTIFY);
GET_REG16(NOR);
GET_REG16(NOR);
_dev->vendor = GET_REG16(NOR + VENDOR);
_dev->device = GET_REG16(NOR + DEVICE);
SET_REG16(NOR + NOR_COMMAND, COMMAND_LOCK);
SET_REG16(NOR, DATA_MODE);
GET_REG16(NOR);
GET_REG16(NOR);
if(_dev->vendor == 0)
{
bufferPrintf("NOR not detected.\n");
return -1;
}
nor_finish(&_dev->mtd);
bufferPrintf("NOR vendor=%x, device=%x\r\n", _dev->vendor, _dev->device);
return mtd_init(&_dev->mtd);
}
static int nor_erase_sector(nor_device_t *_dev, uint32_t _offset)
{
SET_REG16(NOR + NOR_COMMAND, COMMAND_UNLOCK);
SET_REG16(NOR + LOCK, LOCK_UNLOCK);
SET_REG16(NOR + NOR_COMMAND, COMMAND_ERASE);
SET_REG16(NOR + NOR_COMMAND, COMMAND_UNLOCK);
SET_REG16(NOR + LOCK, LOCK_UNLOCK);
SET_REG16(NOR + _offset, ERASE_DATA);
while(TRUE)
{
udelay(50000);
if(GET_REG16(NOR + _offset) != 0xFFFF)
bufferPrintf("failed to erase NOR sector: %x %x\r\n", (unsigned int) GET_REG16(NOR + _offset), GET_REG(0x20000000));
else
break;
}
return 0;
}
static int nor_write_short(nor_device_t *_dev, uint32_t offset, uint16_t data)
{
SET_REG16(NOR + NOR_COMMAND, COMMAND_UNLOCK);
SET_REG16(NOR + LOCK, LOCK_UNLOCK);
SET_REG16(NOR + NOR_COMMAND, COMMAND_WRITE);
SET_REG16(NOR + offset, data);
while(TRUE)
{
udelay(40);
if(GET_REG16(NOR + offset) != data)
bufferPrintf("failed to write to NOR\r\n");
else
break;
}
return 0;
}
void frame_end(uint16 fp, int in_irq) {
unsigned long local, total;
frame_info * frame;
frame_info * pframe;
profile_info *prof, *pprof;
if (!current_thread)
return;
/* If this is the bottom most frame insert a new frame below */
if (current_thread->num_frames == 0)
frame_insert_subframe(fp);
frame = ¤t_thread->frames[current_thread->num_frames];
/* Check if frame pointers match */
while (frame->fp && frame->fp < fp) {
/* This means that the function on top of the stack had no return.
* We just remove it as if it never had been called and reassign
* its cycles to the caller.
*/
(frame-1)->irqcycles += frame->irqcycles;
(frame-1)->threadcycles += frame->threadcycles;
if (--current_thread->num_frames == 0)
frame_insert_subframe(fp);
frame = ¤t_thread->frames[current_thread->num_frames];
}
#ifdef LOG_CALLS
#ifndef LOG_CALLS_IRQ
int i, rec_in_irq = 0;
for (i = current_thread->num_frames; i >= 0; i--)
if (current_thread->frames[i].pc & 1)
rec_in_irq = 1;
if (!rec_in_irq)
#endif
{
fprintf(framelog, "%*s<- %04x\n",
current_thread->num_frames * 3, "",
pc < 0x8000 ? GET_REG16(6) : GET_REG16(0));
}
#endif
current_thread->num_frames--;
/* Compute number of cycles spent in child frame and its children */
total = cycles - frame->startcycle - frame->threadcycles
- frame->irqcycles;
local = total - frame->childcycles;
/* If fp is 0 this means that the return address has been put on stack
* manually. We don't add the cycles of the "child" function in that
* case.
*/
if (frame->fp != 0) {
/* get parent frame */
pframe = frame - 1;
/* Move irq cycles and cycles spent in different threads to parent
* frame */
pframe->irqcycles += frame->irqcycles;
pframe->threadcycles += frame->threadcycles;
/* Add the total number of cycles to child cycles of parent */
if ((frame->pc & 1)) {
/* The function that finished was an interrupt handler */
pframe->irqcycles += total;
} else {
pframe->childcycles += total;
}
#ifdef HASH_PROFILES
pprof = hash_get(&profiles, pframe->pc & ~1);
#else
pprof = profiles[pframe->pc & ~1];
#endif
/* since fp was okay, we know that this function was called regularly
* and we have a profile entry for its caller.
*/
frame_add_callee(pprof, frame->pc, total);
}
#ifdef LOG_CALLS
/* log data structures if a method finished that manipulates them */
#ifdef LOG_TIMERS
if (frame->pc == symbols_getaddr("_add_timer")
|| frame->pc == symbols_getaddr("_remove_timer")
|| frame->pc == symbols_getaddr("_run_timers")) {
extern void bibo_dump_timers(FILE* file);
bibo_dump_timers(framelog);
}
#endif
#ifdef LOG_MEMORY
if (frame->pc == symbols_getaddr("_mm_init")) {
extern void bibo_dump_memory(FILE*);
bibo_dump_memory(framelog);
}
if (frame->pc == symbols_getaddr("_malloc")) {
extern void bibo_dump_memory(FILE*);
bibo_dump_memory(framelog);
}
if (frame->pc == symbols_getaddr("_free")) {
extern void bibo_dump_memory(FILE*);
bibo_dump_memory(framelog);
}
#endif
#ifdef LOG_THREADS
if (frame->pc == symbols_getaddr("_execi")
|| frame->pc == symbols_getaddr("_wait")
|| frame->pc == symbols_getaddr("_make_running")
|| frame->pc == symbols_getaddr("_shutdown_tasks")
|| frame->pc == symbols_getaddr("_wakeup")
|| frame->pc == symbols_getaddr("_wakeup_single")) {
extern void bibo_dump_threads(FILE*);
bibo_dump_threads(framelog);
}
#endif
#endif
/* add profile info for child frame */
#ifdef HASH_PROFILES
prof = hash_get(&profiles, frame->pc & ~1);
#else
prof = profiles[frame->pc & ~1];
#endif
if (!prof) {
#ifdef HASH_PROFILES
prof = hash_create(&profiles, frame->pc & ~1, sizeof(profile_info));
#else
prof = profiles[frame->pc & ~1] = malloc(sizeof(profile_info));
#endif
memset(prof, 0, sizeof(profile_info));
hash_init(&prof->callees, 5);
}
prof->irq_cycles += frame->irqcycles;
prof->local_cycles += local;
prof->total_cycles += total;
prof->calls++;
prof->isIRQ |= (frame->pc & 1);
if (total > prof->max_total_cycles)
prof->max_total_cycles = total;
if (local > prof->max_local_cycles)
prof->max_local_cycles = local;
}
/**
* Handle a call instruction and add profile information for the
* called and calling function.
*/
void frame_begin(uint16 fp, int in_irq) {
frame_info *frame, *pframe;
profile_info *pprof;
/* If we there is no current thread, just return. The current thread
* will be created by the opcode that initializes the stack pointer.
* So there should be always a current thread at the moment we call
* a function.
*/
if (!current_thread)
return;
if (fp < current_thread->minfp)
current_thread->minfp = fp;
/* Add a new frame to the current thread. Resize the stack structure
* if necessary.
*/
frame_grow_num_frames();
/* Initialize the new frame.
*/
frame = ¤t_thread->frames[current_thread->num_frames];
frame->startcycle = cycles;
frame->childcycles = 0;
frame->irqcycles = 0;
frame->threadcycles = 0;
frame->pc = pc | (in_irq ? 1 : 0);
frame->fp = fp;
#ifdef LOG_CALLS
#ifndef LOG_CALLS_IRQ
int i, rec_in_irq = 0;
/* Check if this frame was directly or indirectly called by interrupt. */
for (i = current_thread->num_frames; i >= 0; i--) {
if (current_thread->frames[i].pc & 1) {
rec_in_irq = 1;
break;
}
}
/* Log the call to framelog, unless called from interrupt. */
if (!rec_in_irq)
#endif
{
char* funcname, buf[10];
funcname = symbols_get(pc, 0);
if (!funcname) {
snprintf(buf, 10, "0x%04x", pc);
funcname=buf;
}
fprintf(framelog, "%*s%s(%04x,%04x,%04x,%04x) fp: %04x klock: %d inirq: %d\n",
current_thread->num_frames * 3, "", funcname,
pc < 0x8000 ? GET_REG16(6) : GET_REG16(0),
pc < 0x8000 ? READ_WORD(GET_REG16(7)+2) : GET_REG16(1),
pc < 0x8000 ? READ_WORD(GET_REG16(7)+4) : GET_REG16(2),
pc < 0x8000 ? READ_WORD(GET_REG16(7)+6) : GET_REG16(3),
GET_REG16(7),
memory[symbols_getaddr("_kernel_lock")], in_irq);
}
#endif
/* get parent frame and add a new callee to its profile information. */
pframe = frame - 1;
#ifdef HASH_PROFILES
pprof = hash_get(&profiles, pframe->pc & ~1);
#else
pprof = profiles[pframe->pc & ~1];
#endif
if (!pprof) {
#ifdef HASH_PROFILES
pprof = hash_create(&profiles, pframe->pc & ~1, sizeof(profile_info));
#else
pprof = profiles[pframe->pc & ~1] = malloc(sizeof(profile_info));
#endif
memset(pprof, 0, sizeof(profile_info));
hash_init(&pprof->callees, 5);
}
}
void debug_printf(void) {
char buffer[4096];
size_t maxlen = sizeof(buffer);
int fp = GET_REG16(7)+2;
const char *format = (char*) &memory[GET_WORD(fp)];
fp += 2;
char ch;
long value;
char *strvalue;
int min;
int max;
int state;
int flags;
int cflags;
size_t currlen;
state = DP_S_DEFAULT;
currlen = flags = cflags = min = 0;
max = -1;
ch = *format++;
while (state != DP_S_DONE) {
if (ch == '\0')
state = DP_S_DONE;
switch(state) {
case DP_S_DEFAULT:
if (ch == '%')
state = DP_S_FLAGS;
else
dopr_outch (buffer, &currlen, maxlen, ch);
ch = *format++;
break;
case DP_S_FLAGS:
switch (ch) {
case '-':
flags |= DP_F_MINUS;
ch = *format++;
break;
case '+':
flags |= DP_F_PLUS;
ch = *format++;
break;
case ' ':
flags |= DP_F_SPACE;
ch = *format++;
break;
case '#':
flags |= DP_F_NUM;
ch = *format++;
break;
case '0':
flags |= DP_F_ZERO;
ch = *format++;
break;
default:
state = DP_S_MIN;
break;
}
break;
case DP_S_MIN:
if (isdigit((unsigned char)ch)) {
min = 10*min + char_to_int (ch);
ch = *format++;
} else if (ch == '*') {
min = (short) GET_WORD(fp);
fp += 2;
ch = *format++;
state = DP_S_DOT;
} else {
state = DP_S_DOT;
}
break;
case DP_S_DOT:
if (ch == '.') {
state = DP_S_MAX;
ch = *format++;
} else {
state = DP_S_MOD;
}
break;
case DP_S_MAX:
if (isdigit((unsigned char)ch)) {
if (max < 0)
max = 0;
max = 10*max + char_to_int (ch);
ch = *format++;
} else if (ch == '*') {
min = (short) GET_WORD(fp);
fp += 2;
ch = *format++;
state = DP_S_MOD;
} else {
state = DP_S_MOD;
}
break;
case DP_S_MOD:
switch (ch) {
case 'h':
cflags = DP_C_SHORT;
ch = *format++;
break;
case 'l':
cflags = DP_C_LONG;
ch = *format++;
break;
default:
break;
}
state = DP_S_CONV;
break;
case DP_S_CONV:
switch (ch) {
case 'd':
case 'i':
if (cflags == DP_C_LONG) {
value = ((int) (short) GET_WORD(fp)) << 16
| GET_WORD(fp+2);
fp += 4;
} else {
value = (short) GET_WORD(fp);
fp += 2;
}
fmtint (buffer, &currlen, maxlen, value, 10, min, max, flags);
break;
case 'o':
flags |= DP_F_UNSIGNED;
if (cflags == DP_C_LONG) {
value = ((unsigned int) GET_WORD(fp)) << 16
| GET_WORD(fp+2);
fp += 4;
} else {
value = GET_WORD(fp);
fp += 2;
}
fmtint (buffer, &currlen, maxlen, value, 8, min, max, flags);
break;
case 'u':
flags |= DP_F_UNSIGNED;
if (cflags == DP_C_LONG) {
value = ((unsigned int) GET_WORD(fp)) << 16
| GET_WORD(fp+2);
fp += 4;
} else {
value = GET_WORD(fp);
fp += 2;
}
fmtint (buffer, &currlen, maxlen, value, 10, min, max, flags);
break;
case 'X':
flags |= DP_F_UP;
case 'x':
flags |= DP_F_UNSIGNED;
if (cflags == DP_C_LONG) {
value = ((unsigned int) GET_WORD(fp)) << 16
| GET_WORD(fp+2);
fp += 4;
} else {
value = GET_WORD(fp);
fp += 2;
}
fmtint (buffer, &currlen, maxlen, value, 16, min, max, flags);
break;
case 'c':
dopr_outch (buffer, &currlen, maxlen,
(int)(short)GET_WORD(fp));
fp += 2;
break;
case 's':
{
int addr = GET_WORD(fp);
strvalue = (char*) &memory[addr];
if (!addr)
strvalue = "(NULL)";
if (max == -1) {
max = strlen(strvalue);
}
if (min > 0 && max >= 0 && min > max) max = min;
fmtstr (buffer, &currlen, maxlen, strvalue, flags,
min, max);
}
break;
case 'p':
value = GET_WORD(fp);
fmtint (buffer, &currlen, maxlen, value, 16, min, max, flags);
break;
case '%':
dopr_outch (buffer, &currlen, maxlen, ch);
break;
default:
/* Unknown, skip */
break;
}
ch = *format++;
state = DP_S_DEFAULT;
flags = cflags = min = 0;
max = -1;
break;
case DP_S_DONE:
break;
default:
/* hmm? */
break; /* some picky compilers need this */
}
}
if (maxlen != 0) {
if (currlen < maxlen - 1)
buffer[currlen] = '\0';
else if (maxlen > 0)
buffer[maxlen - 1] = '\0';
}
printf("Program said: `%s'\n", buffer);
}