1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190
|
#include <alloca.h>
#include <linux/kernel.h>
#include <asm/console.h>
#include "hwrpb.h"
#include "system.h"
#include "aboot.h"
#include "cons.h"
#include "utils.h"
#include "string.h"
#ifndef CCB_OPEN_CONSOLE /* new callback w/ ARM v4 */
# define CCB_OPEN_CONSOLE 0x07
#endif
#ifndef CCB_CLOSE_CONSOLE /* new callback w/ ARM v4 */
# define CCB_CLOSE_CONSOLE 0x08
#endif
long cons_dev; /* console device */
long
cons_puts(const char *str, long len)
{
long remaining, written;
union ccb_stsdef {
long int l_sts;
struct {
int written;
unsigned discard : 29;
unsigned v_sts0 : 1;
unsigned v_sts1 : 1;
unsigned v_err : 1;
} s;
} ccb_sts;
for (remaining = len; remaining; remaining -= written) {
ccb_sts.l_sts = dispatch(CCB_PUTS, cons_dev, str, remaining);
if (!ccb_sts.s.v_err) {
written = ccb_sts.s.written;
str += written;
} else {
if (ccb_sts.s.v_sts1)
halt(); /* This is a hard error */
written = 0;
}
}
return len;
}
void
cons_putchar(char c)
{
char buf[2];
buf[0] = c;
buf[1] = 0;
cons_puts(buf,1);
}
int
cons_getchar(void)
{
long c;
while ((c = dispatch(CCB_GETC, cons_dev)) < 0)
;
return c;
}
long
cons_getenv(long index, char *envval, long maxlen)
{
/*
* This may seem silly, but some SRM implementations have
* problems returning values to buffers that are not 8 byte
* aligned. We work around this by always using a buffer
* allocated on the stack (which guaranteed to by 8 byte
* aligned).
*/
char * tmp = alloca(maxlen);
long len;
len = dispatch(CCB_GET_ENV, index, tmp, maxlen - 1);
if (len >= 0) {
memcpy(envval, tmp, len);
envval[len] = '\0';
}
return len;
}
long
cons_open(const char *devname)
{
return dispatch(CCB_OPEN, devname, strlen(devname));
}
long
cons_close(long dev)
{
return dispatch(CCB_CLOSE, dev);
}
long
cons_read(long dev, void *buf, long count, long offset)
{
static char readbuf[SECT_SIZE]; /* minimize frame size */
if ((count & (SECT_SIZE-1)) == 0 && (offset & (SECT_SIZE-1)) == 0) {
/* I/O is aligned... this is easy! */
return dispatch(CCB_READ, dev, count, buf,
offset / SECT_SIZE);
} else {
long bytesleft, iocount, blockoffset, iosize, lbn, retval;
bytesleft = count;
iocount = 0;
blockoffset = offset % SECT_SIZE;
lbn = offset / SECT_SIZE;
while (bytesleft > 0) {
if ((blockoffset == 0) && (bytesleft >= SECT_SIZE)) {
/*
* This portion of the I/O is aligned,
* so read it straight in:
*/
iosize = SECT_SIZE;
retval = dispatch(CCB_READ, dev, iosize, buf,
lbn);
if (retval != iosize) {
printf("read error 0x%lx\n",retval);
return -1;
}
} else {
/*
* Not aligned; must read it into a
* temporary buffer and go from there.
*/
retval = dispatch(CCB_READ, dev, SECT_SIZE,
readbuf, lbn);
if (retval != SECT_SIZE) {
printf("read error, lbn %ld: 0x%lx\n",
lbn, retval);
return -1;
}
iosize = bytesleft;
if (blockoffset + iosize >= SECT_SIZE) {
iosize = SECT_SIZE - blockoffset;
}
memcpy(buf, readbuf + blockoffset, iosize);
}
buf += iosize;
iocount += iosize;
bytesleft -= iosize;
blockoffset = 0;
++lbn;
}
return iocount;
}
}
void cons_open_console(void)
{
dispatch(CCB_OPEN_CONSOLE);
}
void cons_close_console(void)
{
dispatch(CCB_CLOSE_CONSOLE);
}
void
cons_init(void)
{
char envval[256];
if (cons_getenv(ENV_TTY_DEV, envval, sizeof(envval)) < 0) {
halt(); /* better than random crash */
}
cons_dev = simple_strtoul(envval, 0, 10);
cons_open_console();
}
|