515 lines
9.7 KiB
C
515 lines
9.7 KiB
C
#include <string.h>
|
|
#include "serial.h"
|
|
#include "utils.h"
|
|
#include "bionic.h"
|
|
#include "printf.h"
|
|
|
|
//#define AUTOMATED
|
|
|
|
#if !defined(AUTOMATED)
|
|
#define PROMPT "fernly> "
|
|
|
|
static int serial_get_line(char *bfr, int len)
|
|
{
|
|
int cur = 0;
|
|
|
|
while (cur < len) {
|
|
bfr[cur] = serial_getc();
|
|
serial_putc(bfr[cur]);
|
|
|
|
/* Carriage Return */
|
|
if (bfr[cur] == '\n') {
|
|
bfr[cur] = '\0';
|
|
return 0;
|
|
}
|
|
|
|
/* Linefeed */
|
|
else if (bfr[cur] == '\r') {
|
|
bfr[cur] = '\0';
|
|
return 0;
|
|
}
|
|
|
|
/* Backspace */
|
|
else if (bfr[cur] == 0x7f) {
|
|
bfr[cur] = '\0';
|
|
|
|
if (cur > 0) {
|
|
serial_putc('\b');
|
|
serial_putc(' ');
|
|
serial_putc('\b');
|
|
cur--;
|
|
}
|
|
}
|
|
|
|
/* Ctrl-U */
|
|
else if (bfr[cur] == 0x15) {
|
|
while (cur > 0) {
|
|
serial_putc('\b');
|
|
serial_putc(' ');
|
|
serial_putc('\b');
|
|
bfr[cur] = '\0';
|
|
cur--;
|
|
}
|
|
}
|
|
|
|
/* Ctrl-W */
|
|
else if (bfr[cur] == 0x17) {
|
|
while (cur > 0 && bfr[cur] != ' ') {
|
|
serial_putc('\b');
|
|
serial_putc(' ');
|
|
serial_putc('\b');
|
|
bfr[cur] = '\0';
|
|
cur--;
|
|
}
|
|
}
|
|
|
|
/* Escape code */
|
|
else if (bfr[cur] == 0x1b) {
|
|
/* Next two characters are escape codes */
|
|
uint8_t next = serial_getc();
|
|
/* Sanity check: next should be '[' */
|
|
|
|
next = serial_getc();
|
|
}
|
|
else
|
|
cur++;
|
|
}
|
|
bfr[len - 1] = '\0';
|
|
return -1;
|
|
}
|
|
#endif
|
|
|
|
static inline void writeb(uint8_t value, uint32_t addr)
|
|
{
|
|
*((volatile uint8_t *)addr) = value;
|
|
}
|
|
|
|
static inline uint8_t readb(uint32_t addr)
|
|
{
|
|
return *(volatile uint8_t *)addr;
|
|
}
|
|
|
|
static inline void writew(uint16_t value, uint32_t addr)
|
|
{
|
|
*((volatile uint16_t *)addr) = value;
|
|
}
|
|
|
|
static inline uint16_t readw(uint32_t addr)
|
|
{
|
|
return *(volatile uint16_t *)addr;
|
|
}
|
|
|
|
static inline void writel(uint32_t value, uint32_t addr)
|
|
{
|
|
*((volatile uint32_t *)addr) = value;
|
|
}
|
|
|
|
static inline uint32_t readl(uint32_t addr)
|
|
{
|
|
return *(volatile uint32_t *)addr;
|
|
}
|
|
|
|
/*
|
|
static int wdt_kick(void)
|
|
{
|
|
writel(0x1971, 0xa0030008);
|
|
return 0;
|
|
}
|
|
*/
|
|
|
|
static int wdt_reboot(void)
|
|
{
|
|
writel(0x1209, 0xa003001c);
|
|
return 0;
|
|
}
|
|
|
|
static int list_registers(void)
|
|
{
|
|
int var;
|
|
|
|
serial_puts("Registers:\n");
|
|
|
|
serial_puts("CPSR: ");
|
|
asm volatile ("mrs %0, cpsr":"=r" (var));
|
|
serial_puth(var, 8);
|
|
serial_puts("\n");
|
|
|
|
serial_puts("SPSR: ");
|
|
asm volatile ("mrs %0, spsr":"=r" (var));
|
|
serial_puth(var, 8);
|
|
serial_puts("\n");
|
|
|
|
serial_puts("R0: ");
|
|
asm volatile ("mov %0, r0":"=r" (var));
|
|
serial_puth(var, 8);
|
|
serial_puts("\n");
|
|
|
|
serial_puts("R1: ");
|
|
asm volatile ("mov %0, r1":"=r" (var));
|
|
serial_puth(var, 8);
|
|
serial_puts("\n");
|
|
|
|
serial_puts("R2: ");
|
|
asm volatile ("mov %0, r2":"=r" (var));
|
|
serial_puth(var, 8);
|
|
serial_puts("\n");
|
|
|
|
serial_puts("R3: ");
|
|
asm volatile ("mov %0, r3":"=r" (var));
|
|
serial_puth(var, 8);
|
|
serial_puts("\n");
|
|
|
|
serial_puts("R4: ");
|
|
asm volatile ("mov %0, r4":"=r" (var));
|
|
serial_puth(var, 8);
|
|
serial_puts("\n");
|
|
|
|
serial_puts("R5: ");
|
|
asm volatile ("mov %0, r5":"=r" (var));
|
|
serial_puth(var, 8);
|
|
serial_puts("\n");
|
|
|
|
serial_puts("R6: ");
|
|
asm volatile ("mov %0, r6":"=r" (var));
|
|
serial_puth(var, 8);
|
|
serial_puts("\n");
|
|
|
|
serial_puts("R7: ");
|
|
asm volatile ("mov %0, r7":"=r" (var));
|
|
serial_puth(var, 8);
|
|
serial_puts("\n");
|
|
|
|
serial_puts("R8: ");
|
|
asm volatile ("mov %0, r8":"=r" (var));
|
|
serial_puth(var, 8);
|
|
serial_puts("\n");
|
|
|
|
serial_puts("R9: ");
|
|
asm volatile ("mov %0, r9":"=r" (var));
|
|
serial_puth(var, 8);
|
|
serial_puts("\n");
|
|
|
|
serial_puts("R10: ");
|
|
asm volatile ("mov %0, r10":"=r" (var));
|
|
serial_puth(var, 8);
|
|
serial_puts("\n");
|
|
|
|
serial_puts("FP: ");
|
|
asm volatile ("mov %0, r11":"=r" (var));
|
|
serial_puth(var, 8);
|
|
serial_puts("\n");
|
|
|
|
serial_puts("IP: ");
|
|
asm volatile ("mov %0, r12":"=r" (var));
|
|
serial_puth(var, 8);
|
|
serial_puts("\n");
|
|
|
|
serial_puts("SP: ");
|
|
asm volatile ("mov %0, r13":"=r" (var));
|
|
serial_puth(var, 8);
|
|
serial_puts("\n");
|
|
|
|
serial_puts("LR: ");
|
|
asm volatile ("mov %0, r14":"=r" (var));
|
|
serial_puth(var, 8);
|
|
serial_puts("\n");
|
|
|
|
serial_puts("PC: ");
|
|
asm volatile ("mov %0, r15":"=r" (var));
|
|
serial_puth(var, 8);
|
|
serial_puts("\n");
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int enable_irq(void)
|
|
{
|
|
register int var;
|
|
asm volatile ("mrs %0, cpsr":"=r" (var));
|
|
if (!(var & 0x80)) {
|
|
serial_puts("Interrupts already enabled\n");
|
|
return -1;
|
|
}
|
|
|
|
// serial_puts("Interrupts were disabled. Re-enabling...\n");
|
|
var &= ~0x80;
|
|
var &= ~0x1f;
|
|
var |= 0x10;
|
|
asm volatile ("msr cpsr, %0":"=r" (var));
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int enable_fiq(void)
|
|
{
|
|
register int var;
|
|
asm volatile ("mrs %0, cpsr":"=r" (var));
|
|
if (!(var & 0x40)) {
|
|
serial_puts("FIQ already enabled\n");
|
|
return -1;
|
|
}
|
|
|
|
// serial_puts("FIQ was disabled. Re-enabling...\n");
|
|
var &= ~0x40;
|
|
asm volatile ("msr cpsr, %0":"=r" (var));
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int do_init(void)
|
|
{
|
|
void *rv_start = (void *)0x88;
|
|
void *rv_end = (void *)0x88 + 256;
|
|
|
|
list_registers();
|
|
serial_init();
|
|
|
|
/* Kick WDT */
|
|
writel(0x1971, 0xa0030008);
|
|
|
|
/* Disable WDT */
|
|
writel(0x2200, 0xa0030000);
|
|
|
|
serial_puts("\n\nFernly shell\n");
|
|
|
|
/* Copy exception vectors to address 0 */
|
|
_memcpy((void *)0, rv_start, rv_end - rv_start);
|
|
enable_irq();
|
|
enable_fiq();
|
|
|
|
return 0;
|
|
}
|
|
|
|
static inline int get_hex(int bytes)
|
|
{
|
|
uint32_t word = 0;
|
|
uint8_t buf;
|
|
int i;
|
|
|
|
if (bytes == 4)
|
|
i = 28;
|
|
else if (bytes == 2)
|
|
i = 12;
|
|
else
|
|
i = 4;
|
|
|
|
while (i >= 0) {
|
|
buf = serial_getc();
|
|
if (buf > 96)
|
|
buf -= 87;
|
|
else if (buf > 64)
|
|
buf -= 55;
|
|
else
|
|
buf -= 48;
|
|
word |= (buf << i);
|
|
|
|
i -= 4;
|
|
}
|
|
return word;
|
|
}
|
|
|
|
#ifdef AUTOMATED
|
|
/* Protocol:
|
|
* Stream is byte-oriented. The following commands are known:
|
|
*
|
|
* r - read an address
|
|
* w - write to an address
|
|
* z - zero-fill a region
|
|
*
|
|
* Responses:
|
|
*
|
|
* k - Ready for command
|
|
* ? - Unknown command
|
|
*/
|
|
static int loop(void)
|
|
{
|
|
int buf;
|
|
int size;
|
|
uint32_t offset;
|
|
uint32_t value;
|
|
|
|
serial_putc('k');
|
|
buf = serial_getc();
|
|
|
|
switch (buf) {
|
|
/* Read. Format: r[otf]aaaaaaaa
|
|
otf -> read One, Two, or Four bytes
|
|
a.. -> address to read
|
|
*/
|
|
case 'r':
|
|
size = serial_getc();
|
|
if (size == 'o') {
|
|
offset = get_hex(4);
|
|
value = readb(offset);
|
|
serial_puth(value, 2);
|
|
}
|
|
else if (size == 't') {
|
|
offset = get_hex(4);
|
|
value = readw(offset);
|
|
serial_puth(value, 4);
|
|
}
|
|
else {
|
|
offset = get_hex(4);
|
|
value = readl(offset);
|
|
serial_puth(value, 8);
|
|
}
|
|
break;
|
|
|
|
/* Write. Format: w[otf]aaaaaaaavvvvvvvv
|
|
otf -> write One, Two, or Four bytes
|
|
a.. -> address to write
|
|
v.. -> value to write
|
|
*/
|
|
case 'w':
|
|
size = serial_getc();
|
|
if (size == 'o') {
|
|
offset = get_hex(4);
|
|
value = get_hex(1);
|
|
writeb(value, offset);
|
|
serial_puth(value, 2);
|
|
}
|
|
else if (size == 't') {
|
|
offset = get_hex(4);
|
|
value = get_hex(2);
|
|
writew(value, offset);
|
|
serial_puth(value, 4);
|
|
}
|
|
else {
|
|
offset = get_hex(4);
|
|
value = get_hex(4);
|
|
writel(value, offset);
|
|
serial_puth(value, 8);
|
|
}
|
|
break;
|
|
|
|
case 'z': {
|
|
uint32_t start;
|
|
uint32_t end;
|
|
|
|
start = get_hex(4);
|
|
end = get_hex(4);
|
|
while (start < end) {
|
|
*((uint32_t *)start) = 0;
|
|
start += 4;
|
|
}
|
|
}
|
|
break;
|
|
|
|
default:
|
|
serial_putc('?');
|
|
break;
|
|
}
|
|
return 0;
|
|
}
|
|
#else /* AUTOMATED */
|
|
|
|
static int cmd_help(int argc, char **argv);
|
|
static int cmd_reboot(int argc, char **argv);
|
|
static int cmd_args(int argc, char **argv);
|
|
|
|
static struct {
|
|
int (*func)(int argc, char **argv);
|
|
const char *name;
|
|
const char *help;
|
|
} commands[] = {
|
|
{
|
|
.func = cmd_help,
|
|
.name = "help",
|
|
.help = "Print help about available commands",
|
|
},
|
|
{
|
|
.func = cmd_reboot,
|
|
.name = "reboot",
|
|
.help = "Reboot Fernvale",
|
|
},
|
|
{
|
|
.func = cmd_args,
|
|
.name = "args",
|
|
.help = "Print contents of arc and argv",
|
|
},
|
|
};
|
|
|
|
int cmd_help(int argc, char **argv)
|
|
{
|
|
int i;
|
|
|
|
for (i = 0; i < sizeof(commands) / sizeof(*commands); i++) {
|
|
serial_puts(commands[i].name);
|
|
serial_putc('\t');
|
|
serial_puts(commands[i].help);
|
|
serial_puts("\n");
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int cmd_args(int argc, char **argv)
|
|
{
|
|
int i;
|
|
printf("argc: %d\n", argc);
|
|
for (i = 0; i < argc; i++) {
|
|
printf("argv[%d]: ", i);
|
|
serial_puts(argv[i]);
|
|
serial_puts("\n");
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int cmd_reboot(int argc, char **argv)
|
|
{
|
|
printf("Rebooting...\n");
|
|
wdt_reboot();
|
|
while(1);
|
|
return 0;
|
|
}
|
|
|
|
static int shell_run_command(char *line)
|
|
{
|
|
char *lp, *cmd, *tokp;
|
|
char *args[8];
|
|
int i, n;
|
|
|
|
lp = _strtok(line, " \t", &tokp);
|
|
cmd = lp;
|
|
n = 0;
|
|
while ((lp = _strtok(NULL, " \t", &tokp)) != NULL) {
|
|
if (n >= 7) {
|
|
printf("too many arguments\r\n");
|
|
cmd = NULL;
|
|
break;
|
|
}
|
|
args[n++] = lp;
|
|
}
|
|
args[n] = NULL;
|
|
if (cmd == NULL)
|
|
return -1;
|
|
|
|
for (i = 0; i < sizeof(commands) / sizeof(*commands); i++)
|
|
if (!_strcasecmp(commands[i].name, cmd))
|
|
return commands[i].func(n, args);
|
|
|
|
printf("Unknown command: %s\n", args[0]);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int loop(void)
|
|
{
|
|
char line[256];
|
|
|
|
serial_puts(PROMPT);
|
|
serial_get_line(line, sizeof(line));
|
|
printf("\n");
|
|
return shell_run_command(line);
|
|
}
|
|
#endif
|
|
|
|
int main(void)
|
|
{
|
|
do_init();
|
|
|
|
while (1)
|
|
loop();
|
|
|
|
return 0;
|
|
}
|