unsigned char
fob_errorcode(void)
{
sio_putc(0x4F); // Examine Value (FoBman p.73)
sio_putc(0x0a); // Error Code (FoBman p.119, p135, p149)
return sio_getc();
}
void
fob_hemisphere(unsigned char axis, unsigned char sign)
{
sio_putc(0x4C); // stream stop (FoBman p.76)
sio_putc(axis);
sio_putc(sign);
}
/* Output a character over the serial port. */
int sbcall_putc(tge_sbcall_putc_arg_t *arg)
{
/* Translate \n to \r\n. */
if (arg->c == '\n') {
sio_putc('\r');
return sio_putc('\n');
}
return sio_putc(arg->c);
}
int sio_puts(const char *str)
{
int res;
res = sio_putsn(str);
sio_putc('\r');
sio_putc('\n');
return res + 2;
}
void sio_puts(const char *s)
{
while(*s != 0) {
sio_putc(*s);
s++;
}
}
void
fob_status_print(void)
{
unsigned int retval1, retval2, retval, status;
int num, i;
// Bird Status
sio_putc(0x4F); // Examine Value (FoBman p.73)
sio_putc(0); // Bird Status (FoBman p.112)
retval1 = sio_getc();
retval2 = sio_getc();
status = ((retval2 & 0xff) << 8) | (retval1 & 0xff);
printf("status: %04x\n", status);
printf("master?: %c\n", (status & 0x8000) ? 'O' : 'X');
printf("init?: %c\n", (status & 0x4000) ? 'O' : 'X');
printf("error?: %c\n", (status & 0x2000) ? 'O' : 'X');
printf("running?: %c\n", (status & 0x1000) ? 'O' : 'X');
printf("hostsync?: %c\n", (status & 0x800) ? 'O' : 'X');
printf("expanded address mode?: %c\n", (status & 0x400) ? 'O' : 'X');
printf("crtSync?: %c\n", (status & 0x200) ? 'O' : 'X');
printf("noSync?: %c\n", (status & 0x100) ? 'O' : 'X');
printf("factory test?: %c\n", (status & 0x80) ? 'O' : 'X');
printf("XOFF?: %c\n", (status & 0x40) ? 'O' : 'X');
printf("sleep?: %c\n", (status & 0x20) ? 'O' : 'X');
printf("stream?: %c\n", (status & 0x1) ? 'O' : 'X');
// Flock System Status
sio_putc(0x4F); // Examine Value (FoBman p.73)
sio_putc(36); // Flock System Status mode (FoBman p.130)
printf("Flock System Status (max %d birds)\n", fob_maxaddrs);
for (i = 0; i < fob_maxaddrs; ++i) {
retval = sio_getc();
printf("%3d: %s, %s, %s, %s, %s, %s, %s, %s\n", i,
(retval & 0x80) ? "ACCESSIBLE" : "not accessible",
(retval & 0x40) ? "RUNNING" : "not running",
(retval & 0x20) ? "a SENSOR exists" : "no sensors",
(retval & 0x10) ? "ERT" : "not ert",
(retval & 0x8) ? "ERT#3" : "no ert#3",
(retval & 0x4) ? "ERT#2" : "no ert#2",
(retval & 0x2) ? "ERT#1" : "no ert#1",
(retval & 0x1) ? "ERT#0 or SRT" : "no ert#0 nor srt");
}
}
void sio_putc(const char c)
{
if (c == '\n')
sio_putc('\r');
while ((inl(EE_SIO_ISR) & ISR_TXFIFO_MASK) == ISR_TXFIFO_FULL) {
/* Wait until data can be send. */
};
outb(c, EE_SIO_TXFIFO);
}
int sio_putsn(const char *str)
{
int res;
for (res = 0; *str; res++, str++)
sio_putc(*str);
return res;
}
void
xputc(int c)
{
if (ioctrl & IO_KEYBOARD)
putc(c);
if (ioctrl & IO_SERIAL)
sio_putc(c);
}
void sioprintf(const char *zFormat, ...) {
va_list ap;
char resStr[2048];
va_start(ap,zFormat);
vsnprintf(resStr, 2048, zFormat, ap);
va_end(ap);
char *pos = resStr;
while (*pos) {
if (*pos == '\n') {
// SIO terminal needs explicit CRLF
sio_putc('\r');
sio_putc('\n');
} else
sio_putc(*pos);
pos++;
}
}
/* Not really much to do in the way of error-handling. sio_putc() already
checks to see if there's room in the TX FIFO, and my cable doesn't support
hardware flow control. */
size_t sio_write(const char *buf, size_t size)
{
const char *p = (const char *)buf;
size_t i;
for (i = 0; i < size; i++)
sio_putc(p[i]);
return size;
}
void sioprintf(const char *zFormat, ...) {
#if 0 // doesn't seem to work with ps2link...
va_list ap;
char resStr[2048];
va_start(ap,zFormat);
vsnprintf(resStr, 2048, zFormat, ap);
va_end(ap);
char *pos = resStr;
while (*pos) {
if (*pos == '\n') {
// SIO terminal needs explicit CRLF
sio_putc('\r');
sio_putc('\n');
} else
sio_putc(*pos);
pos++;
}
#endif
}
int sio_putc(int c)
{
if((c == '\n') && (___last_sio_putc != '\r'))
{
// hack: if the character to be outputted is a '\n'
// and the previously-outputted character is not a '\r',
// output a '\r' first.
sio_putc('\r');
}
/* Block until we're ready to transmit. */
while ((_lw(SIO_ISR) & 0xf000) == 0x8000);
_sb(c, SIO_TXFIFO);
___last_sio_putc = c;
return c;
}
/*
**
** [func] - fwrite.
** [desc] - attempts to write n number of records of r size to the stream file
** and returns the number of records successfully written to the file.
** [entr] - const void *buf; the pointer to the source data buffer.
** size_t r; the size of the records to write.
** size_t n the number of records to write.
** FILE *stream; the pointer to the FILE stream.
** [exit] - size_t; the number of records successfully written to the stream file.
** [prec] - buf is a valid memory pointer of (r * n) size in bytes and stream
** is a valid FILE pointer.
** [post] - the stream file is modified.
**
*/
size_t fwrite(const void *buf, size_t r, size_t n, FILE *stream)
{
size_t i, len, ret;
int written = 0;
if (stream->has_putback)
{
fseek(stream, -1, SEEK_CUR);
stream->has_putback = 0;
}
switch(stream->type) {
case STD_IOBUF_TYPE_GS:
/* write to stdout. */
for (i = 0, len = (r * n); i < len; ++i) putchar((int)((char *)buf)[i]);
ret = r;
break;
case STD_IOBUF_TYPE_STDOUTHOST:
//ret = (fioWrite(1, (void *) buf, (int)(r * n)) / (int)r);
written = (_ps2sdk_write(1, (void *) buf, (int)(r * n)) / (int)r);
break;
case STD_IOBUF_TYPE_SIO:
for (i = 0, len = (r * n); i < len; ++i) sio_putc((int)((char *)buf)[i]);
ret = r;
break;
default:
/* attempt to write the stream file. */
//ret = (fioWrite(stream->fd, (void *)buf, (int)(r * n)) / (int)r);
written = (_ps2sdk_write(stream->fd, (void *)buf, (int)(r * n)) / (int)r);
}
if (written < 0) {
/* _ps2sdk_write returns negative errno on error */
errno = (written * -1);
ret = 0;
}
else {
ret = written;
}
return (ret);
}
void
fob_autoconfig(char *hemisphere)
{
int i;
unsigned char axis = 0, sign = 0;
sio_putc(0x50); // Change Value
sio_putc(0x32); // FBB Auto-Configuration p.131
sio_putc(fob_num_birds);
//sio_putc(3);
sio_flush();
if (hemisphere) {
switch (hemisphere[0]) {
case 'f':
break;
case 'l':
axis = 6; sign = 1;
break;
default:
break;
}
}
usleep(700000);
for (i = 0; i < fob_num_sensors; ++i) {
fob_RS232toFOB(fob_sensors[i]);
sio_putc(0x59); // Position/Angles (FoBman p.90)
fob_RS232toFOB(fob_sensors[i]);
fob_hemisphere(axis, sign);
}
sio_putc(0x50); // Change Value
sio_putc(0x23); // Group mode (p.129);
sio_putc(1);
}
void
fob_stream_stop(void)
{
sio_putc(0x3F); // stream stop (FoBman p.103)
}
void
fob_fbbreset(void)
{
sio_putc(0x2F); // FBB Reset (FoBman p.75)
}
void
fob_stream(void)
{
sio_putc(0x40); // Stream (p.101)
}
void
fob_RS232toFOB(unsigned char addr)
{
sio_putc(0xf0 | addr);
}
void
fob_open(char *fn, speed_t speed, int usec)
{
int fd;
//char *fn = "/dev/ttyS0";
unsigned int retval1, retval2, retval, status;
int num, i;
char strbuf[1024], *str;
if (-1 == (fd = open(fn, O_RDONLY))) {
perror(fn);
exit(1);
}
rtsclear(fd);
close(fd);
usleep(usec);
fd = sio_init(fn, speed);
rtsclear(0);
// Bird Status
sio_putc(0x4F); // Examine Value (FoBman p.73)
sio_putc(0); // Bird Status (FoBman p.112)
retval1 = sio_getc();
retval2 = sio_getc();
status = ((retval2 & 0xff) << 8) | (retval1 & 0xff);
#if 0
printf("status: %04x\n", status);
printf("master?: %c\n", (status & 0x8000) ? 'O' : 'X');
printf("init?: %c\n", (status & 0x4000) ? 'O' : 'X');
printf("error?: %c\n", (status & 0x2000) ? 'O' : 'X');
printf("running?: %c\n", (status & 0x1000) ? 'O' : 'X');
printf("hostsync?: %c\n", (status & 0x800) ? 'O' : 'X');
printf("expanded address mode?: %c\n", (status & 0x400) ? 'O' : 'X');
printf("crtSync?: %c\n", (status & 0x200) ? 'O' : 'X');
printf("noSync?: %c\n", (status & 0x100) ? 'O' : 'X');
printf("factory test?: %c\n", (status & 0x80) ? 'O' : 'X');
printf("XOFF?: %c\n", (status & 0x40) ? 'O' : 'X');
printf("sleep?: %c\n", (status & 0x20) ? 'O' : 'X');
printf("stream?: %c\n", (status & 0x1) ? 'O' : 'X');
#endif
if (0 == (status & 0x8000)) {
fprintf(stderr, "FoB Hardware Config Error: not master\n");
exit(1);
}
// FBB Addressing mode
sio_putc(0x4F); // Examine Value (FoBman p.73)
sio_putc(19); // FBB Addressing mode (FoBman p.123)
retval1 = sio_getc();
str = "Unknown";
switch (retval1) {
case 0:
str = "Normal";
num = 14;
break;
case 1:
str = "Expanded";
num = 30;
break;
case 3:
str = "SuperExpanded";
num = 126;
break;
default:
fprintf(stderr, "FATAL ERROR: unknown addressing mode\n");
exit(1);
break;
}
fob_addrmode = retval1;
fob_maxaddrs = num;
fob_num_sensors = 0;
printf("FBB addressing mode: %s (%x)\n", str, retval1);
// Flock System Status
sio_putc(0x4F); // Examine Value (FoBman p.73)
sio_putc(36); // Flock System Status mode (FoBman p.130)
//printf("Flock System Status (max %d birds)\n", num);
for (i = 0; i < num; ++i) {
retval = sio_getc();
#if 0
printf("%3d: %s, %s, %s, %s, %s, %s, %s, %s\n", i,
(retval & 0x80) ? "ACCESSIBLE" : "not accessible",
(retval & 0x40) ? "RUNNING" : "not running",
(retval & 0x20) ? "a SENSOR exists" : "no sensors",
(retval & 0x10) ? "ERT" : "not ert",
(retval & 0x8) ? "ERT#3" : "no ert#3",
(retval & 0x4) ? "ERT#2" : "no ert#2",
(retval & 0x2) ? "ERT#1" : "no ert#1",
(retval & 0x1) ? "ERT#0 or SRT" : "no ert#0 nor srt");
#endif
if (retval & 0x80) {
++fob_num_birds;
}
if (retval & 0x20) {
fob_sensors[fob_num_sensors] = i + 1;
++fob_num_sensors;
}
}
#if 0
printf("fob_addrmode: %d\n", fob_addrmode);
printf("fob_maxaddrs: %d\n", fob_maxaddrs);
printf("fob_num_sensors: %d\n", fob_num_sensors);
for (i = 0; i < fob_num_sensors; ++i) {
printf("fob_sensors[%d]: %d\n", i, fob_sensors[i]);
}
#endif
printf("fob_num_birds: %d\n", fob_num_birds);
}
static void * sior_rpc_server(u32 funcno, void * data, int size) {
int res = 0, c;
size_t s;
char * p;
struct init_arguments_t * i;
switch(funcno) {
case SIOR_INIT:
i = (struct init_arguments_t *) data;
sio_init(i->baudrate, i->lcr_ueps, i->lcr_upen, i->lcr_usbl, i->lcr_umode);
break;
case SIOR_PUTC:
c = *((int *) data);
res = sio_putc(c);
break;
case SIOR_GETC:
res = sio_getc();
break;
case SIOR_GETCBLOCK:
res = sio_getc_block();
break;
case SIOR_WRITE:
p = *((char **) data) + IOP_MEM;
s = *(((size_t *) data) + 1);
DI();
ee_kmode_enter();
res = sio_write(p, s);
ee_kmode_exit();
EI();
break;
case SIOR_READ:
p = *((char **) data) + IOP_MEM;
s = *(((size_t *) data) + 1);
DI();
ee_kmode_enter();
res = sio_read(p, s);
ee_kmode_exit();
EI();
break;
case SIOR_PUTS:
p = *((char **) data) + IOP_MEM;
DI();
ee_kmode_enter();
res = sio_puts(p);
ee_kmode_exit();
EI();
break;
case SIOR_PUTSN:
p = *((char **) data) + IOP_MEM;
DI();
ee_kmode_enter();
res = sio_putsn(p);
ee_kmode_exit();
EI();
break;
case SIOR_GETS:
p = *((char **) data) + IOP_MEM;
DI();
ee_kmode_enter();
(char*)res = sio_gets(p);
ee_kmode_exit();
EI();
break;
case SIOR_FLUSH:
sio_flush();
break;
}
*((int *) data) = res;
return data;
}