From e17cb22b97495a39b5c2a1e4cf0bbe0f4fb8a185 Mon Sep 17 00:00:00 2001 From: Alexander Couzens Date: Sun, 29 Oct 2017 02:51:44 +0200 Subject: [PATCH] rework sserialice Change-Id: I3d61bd21589d7d8f5e23b28d6c86ffd2a23fde98 --- src/console/serialice.c | 188 ++++++++++++++++++++++------------------ 1 file changed, 104 insertions(+), 84 deletions(-) diff --git a/src/console/serialice.c b/src/console/serialice.c index ae4d6fe97e..f4c4763975 100644 --- a/src/console/serialice.c +++ b/src/console/serialice.c @@ -97,10 +97,9 @@ static void sio_put32(u32 data) sio_flush(); } -static u8 sio_get_nibble(void) +static u8 sio_get_nibble(char nibble) { u8 ret = 0; - u8 nibble = sio_getc(); if (nibble >= '0' && nibble <= '9') { ret = (nibble - '0'); @@ -114,68 +113,71 @@ static u8 sio_get_nibble(void) return ret; } -static u8 sio_get8(void) +/* caller must ensure 2 bytes given */ +static u8 sio_get8(char *buffer) { u8 data; - data = sio_get_nibble(); + data = sio_get_nibble(buffer[0]); data = data << 4; - data |= sio_get_nibble(); + data |= sio_get_nibble(buffer[1]); return data; } -static u16 sio_get16(void) +/* caller must ensure 4 bytes given */ +static u16 sio_get16(char *buffer) { u16 data; - data = sio_get_nibble(); + data = sio_get_nibble(buffer[0]); data = data << 4; - data |= sio_get_nibble(); + data |= sio_get_nibble(buffer[1]); data = data << 4; - data |= sio_get_nibble(); + data |= sio_get_nibble(buffer[2]); data = data << 4; - data |= sio_get_nibble(); + data |= sio_get_nibble(buffer[3]); return data; } -static u32 sio_get32(void) +static u32 sio_get32(char *buffer) { u32 data; - data = sio_get_nibble(); + data = sio_get_nibble(buffer[0]); data = data << 4; - data |= sio_get_nibble(); + data |= sio_get_nibble(buffer[1]); data = data << 4; - data |= sio_get_nibble(); + data |= sio_get_nibble(buffer[2]); data = data << 4; - data |= sio_get_nibble(); + data |= sio_get_nibble(buffer[3]); data = data << 4; - data |= sio_get_nibble(); + data |= sio_get_nibble(buffer[4]); data = data << 4; - data |= sio_get_nibble(); + data |= sio_get_nibble(buffer[5]); data = data << 4; - data |= sio_get_nibble(); + data |= sio_get_nibble(buffer[6]); data = data << 4; - data |= sio_get_nibble(); + data |= sio_get_nibble(buffer[7]); return data; } /* SerialICE interface functions */ -static void serialice_read_memory(void) +static void serialice_read_memory(char *buffer, int len) { u8 width; u32 *addr; + if (len < 13) { + sio_putstring("Wrong size\n"); + } + // Format: // *rm00000000.w - addr = (u32 *)sio_get32(); - sio_getc(); // skip . - width = sio_getc(); - - sio_putc('\r'); sio_putc('\n'); + addr = (u32 *)sio_get32(buffer + 3); + width = buffer[12]; switch (width) { case 'b': @@ -190,7 +192,7 @@ static void serialice_read_memory(void) } } -static void serialice_write_memory(void) +static void serialice_write_memory(char *buffer, int len) { u8 width; u32 *addr; @@ -198,39 +200,34 @@ static void serialice_write_memory(void) // Format: // *wm00000000.w=0000 - addr = (u32 *)sio_get32(); - sio_getc(); // skip . - width = sio_getc(); - sio_getc(); // skip = + addr = (u32 *)sio_get32(buffer + 3); + width = buffer[12]; switch (width) { case 'b': - data = sio_get8(); + data = sio_get8(buffer + 14); write8(addr, (u8)data); break; case 'w': - data = sio_get16(); + data = sio_get16(buffer + 14); write16(addr, (u16)data); break; case 'l': - data = sio_get32(); + data = sio_get32(buffer + 14); write32(addr, (u32)data); break; } } -static void serialice_read_io(void) +static void serialice_read_io(char *buffer, int len) { u8 width; u16 port; // Format: // *ri0000.w - port = sio_get16(); - sio_getc(); // skip . - width = sio_getc(); - - sio_putc('\r'); sio_putc('\n'); + port = sio_get16(buffer + 3); + width = buffer[8]; switch (width) { case 'b': @@ -245,47 +242,52 @@ static void serialice_read_io(void) } } -static void serialice_write_io(void) +static void serialice_write_io(char *buffer, int len) { u8 width; u16 port; u32 data; + if (len < 14) { + sio_putstring("failed write_io"); + return; + } + // Format: // *wi0000.w=0000 - port = sio_get16(); - sio_getc(); // skip . - width = sio_getc(); - sio_getc(); // skip = + port = sio_get16(buffer + 3); + width = buffer[8]; switch (width) { case 'b': - data = sio_get8(); + data = sio_get8(buffer + 10); outb((u8)data, port); break; case 'w': - data = sio_get16(); + data = sio_get16(buffer + 10); outw((u16)data, port); break; case 'l': - data = sio_get32(); + data = sio_get32(buffer + 10); outl((u32)data, port); break; } } -static void serialice_read_msr(void) +static void serialice_read_msr(char *buffer, int len) { u32 addr, key; msr_t msr; + if (len < 20) { + sio_putstring("read msr to small"); + return; + } + // Format: // *rc00000000.9c5a203a - addr = sio_get32(); - sio_getc(); // skip . - key = sio_get32(); // key in %edi - - sio_putc('\r'); sio_putc('\n'); + addr = sio_get32(buffer + 3); + key = sio_get32(buffer + 12); // key in %edi msr = rdmsr(addr, key); sio_put32(msr.hi); @@ -293,20 +295,22 @@ static void serialice_read_msr(void) sio_put32(msr.lo); } -static void serialice_write_msr(void) +static void serialice_write_msr(char *buffer, int len) { u32 addr, key; msr_t msr; + if (len < 38) { + sio_putstring("write msr to small"); + return; + } + // Format: // *wc00000000.9c5a203a=00000000.00000000 - addr = sio_get32(); - sio_getc(); // skip . - key = sio_get32(); // read key in %edi - sio_getc(); // skip = - msr.hi = sio_get32(); - sio_getc(); // skip . - msr.lo = sio_get32(); + addr = sio_get32(buffer + 3); + key = sio_get32(buffer + 12); // read key in %edi + msr.hi = sio_get32(buffer + 21); + msr.lo = sio_get32(buffer + 30); #ifdef __ROMCC__ /* Cheat to avoid register outage */ @@ -316,7 +320,7 @@ static void serialice_write_msr(void) #endif } -static void serialice_cpuinfo(void) +static void serialice_cpuinfo(char *buffer, int len) { u32 eax, ecx; u32 reg32; @@ -324,11 +328,8 @@ static void serialice_cpuinfo(void) // Format: // --EAX--- --ECX--- // *ci00000000.00000000 - eax = sio_get32(); - sio_getc(); // skip . - ecx = sio_get32(); - - sio_putc('\r'); sio_putc('\n'); + eax = sio_get32(buffer + 3); + ecx = sio_get32(buffer + 12); /* This code looks quite crappy but this way we don't * have to worry about running out of registers if we @@ -350,14 +351,12 @@ static void serialice_cpuinfo(void) sio_put32(reg32); } -static void serialice_mainboard(void) +static void serialice_mainboard(char *buffer, int len) { int i = 0; const char mb_string[] = CONFIG_MAINBOARD_VENDOR" " CONFIG_MAINBOARD_PART_NUMBER; - sio_putc('\r'); sio_putc('\n'); - while (i < 32 && mb_string[i] > 0) { sio_putc(mb_string[i]); i++; @@ -370,54 +369,75 @@ static void serialice_mainboard(void) sio_flush(); } -static void serialice_version(void) +static void serialice_version(char *buffer, int len) { sio_putstring("\nSerialICE v" VERSION "\n"); } +static int sio_get_line(char *buffer, int len) +{ + int i; + + for (i=0; i "); - c = sio_getc(); + len = sio_get_line(line, sizeof(line)); + /* only new line read */ + if (len == 0) + continue; + + c = line[0]; if (c != '*') continue; - c = sio_getc() << 8; - c |= sio_getc(); + c = line[1] << 8 | line[2]; switch (c) { case (('r' << 8)|'m'): // Read Memory *rm - serialice_read_memory(); + serialice_read_memory(line, len); break; case (('w' << 8)|'m'): // Write Memory *wm - serialice_write_memory(); + serialice_write_memory(line, len); break; case (('r' << 8)|'i'): // Read IO *ri - serialice_read_io(); + serialice_read_io(line, len); break; case (('w' << 8)|'i'): // Write IO *wi - serialice_write_io(); + serialice_write_io(line, len); break; case (('r' << 8)|'c'): // Read CPU MSR *rc - serialice_read_msr(); + serialice_read_msr(line, len); break; case (('w' << 8)|'c'): // Write CPU MSR *wc - serialice_write_msr(); + serialice_write_msr(line, len); break; case (('c' << 8)|'i'): // Read CPUID *ci - serialice_cpuinfo(); + serialice_cpuinfo(line, len); break; case (('m' << 8)|'b'): // Read mainboard type *mb - serialice_mainboard(); + serialice_mainboard(line, len); break; case (('v' << 8)|'i'): // Read version info *vi - serialice_version(); + serialice_version(line, len); break; case ('?'): sio_putstring(""