static inline void
push_replacement(SerdReader* reader, Ref dest)
{
push_byte(reader, dest, 0xEF);
push_byte(reader, dest, 0xBF);
push_byte(reader, dest, 0xBD);
}
/***************************************************************
Return the current state of all parts of robot
***************************************************************/
void return_all()
{
//printf(" in return_all_sensors() ");
start_checksum();
push_char(0x50);
push_char(0xAF);
push_char(0x81);
push_word(get_sonar(1));
push_word(get_sonar(2));
push_word(get_sonar(3));
push_word(get_line_follower(1));
push_word(get_line_follower(2));
push_word(get_line_follower(3));
push_word(get_gyro());
push_byte(get_bumper(1)); # check new spec
push_byte(get_bumper(2));
push_byte(get_motor(1));
push_byte(get_motor(2));
push_byte(get_motor(3));
send_checksum();
// Now 3 + 6 + 6 + 2 + 2 + 3 + 1 = 23
}
static void push_irq_registers(struct MC6809 *cpu) {
NVMA_CYCLE;
push_word(cpu, ®_S, REG_PC);
push_word(cpu, ®_S, REG_U);
push_word(cpu, ®_S, REG_Y);
push_word(cpu, ®_S, REG_X);
push_byte(cpu, ®_S, REG_DP);
push_byte(cpu, ®_S, RREG_B);
push_byte(cpu, ®_S, RREG_A);
push_byte(cpu, ®_S, REG_CC);
}
static bool
read_PERCENT(SerdReader* reader, Ref dest)
{
push_byte(reader, dest, eat_byte_safe(reader, '%'));
const uint8_t h1 = read_HEX(reader);
const uint8_t h2 = read_HEX(reader);
if (h1 && h2) {
push_byte(reader, dest, h1);
push_byte(reader, dest, h2);
return true;
}
return false;
}
int push_data(int serv_pid, char *str, int pid_cond)
{
int i;
i = 0;
while (str[i] != 0)
{
push_byte(str[i], serv_pid, pid_cond);
i++;
}
push_byte(str[i], serv_pid, pid_cond);
return (0);
}
static void psh(struct MC6809 *cpu, uint16_t *s, uint16_t as) {
unsigned postbyte;
postbyte = byte_immediate(cpu);
NVMA_CYCLE;
NVMA_CYCLE;
peek_byte(cpu, *s);
if (postbyte & 0x80) { push_word(cpu, s, REG_PC); }
if (postbyte & 0x40) { push_word(cpu, s, as); }
if (postbyte & 0x20) { push_word(cpu, s, REG_Y); }
if (postbyte & 0x10) { push_word(cpu, s, REG_X); }
if (postbyte & 0x08) { push_byte(cpu, s, REG_DP); }
if (postbyte & 0x04) { push_byte(cpu, s, RREG_B); }
if (postbyte & 0x02) { push_byte(cpu, s, RREG_A); }
if (postbyte & 0x01) { push_byte(cpu, s, REG_CC); }
}
static Ref
read_IRIREF(SerdReader* reader)
{
TRY_RET(eat_byte_check(reader, '<'));
Ref ref = push_node(reader, SERD_URI, "", 0);
uint32_t code;
while (true) {
const uint8_t c = peek_byte(reader);
switch (c) {
case '"': case '<': case '^': case '`': case '{': case '|': case '}':
r_err(reader, SERD_ERR_BAD_SYNTAX,
"invalid IRI character `%c'\n", c);
return pop_node(reader, ref);
case '>':
eat_byte_safe(reader, c);
return ref;
case '\\':
eat_byte_safe(reader, c);
if (!read_UCHAR(reader, ref, &code)) {
r_err(reader, SERD_ERR_BAD_SYNTAX, "invalid IRI escape\n");
return pop_node(reader, ref);
}
switch (code) {
case 0: case ' ': case '<': case '>':
r_err(reader, SERD_ERR_BAD_SYNTAX,
"invalid escaped IRI character %X %c\n", code, code);
return pop_node(reader, ref);
}
break;
default:
if (c <= 0x20) {
if (isprint(c)) {
r_err(reader, SERD_ERR_BAD_SYNTAX,
"invalid IRI character `%c' (escape %%%02X)\n", c, c);
} else {
r_err(reader, SERD_ERR_BAD_SYNTAX,
"invalid IRI character (escape %%%02X)\n", c, c);
}
if (reader->strict) {
return pop_node(reader, ref);
}
push_byte(reader, ref, eat_byte_safe(reader, c));
} else {
push_byte(reader, ref, eat_byte_safe(reader, c));
}
}
}
}
static Ref
read_BLANK_NODE_LABEL(SerdReader* reader, bool* ate_dot)
{
eat_byte_safe(reader, '_');
eat_byte_check(reader, ':');
Ref ref = push_node(reader, SERD_BLANK,
reader->bprefix ? (char*)reader->bprefix : "",
reader->bprefix_len);
uint8_t c = peek_byte(reader); // First: (PN_CHARS | '_' | [0-9])
if (is_digit(c) || c == '_') {
push_byte(reader, ref, eat_byte_safe(reader, c));
} else if (!read_PN_CHARS(reader, ref)) {
r_err(reader, SERD_ERR_BAD_SYNTAX, "invalid name start character\n");
return pop_node(reader, ref);
}
while ((c = peek_byte(reader))) { // Middle: (PN_CHARS | '.')*
if (c == '.') {
push_byte(reader, ref, eat_byte_safe(reader, c));
} else if (!read_PN_CHARS(reader, ref)) {
break;
}
}
SerdNode* n = deref(reader, ref);
if (n->buf[n->n_bytes - 1] == '.' && !read_PN_CHARS(reader, ref)) {
// Ate trailing dot, pop it from stack/node and inform caller
--n->n_bytes;
serd_stack_pop(&reader->stack, 1);
*ate_dot = true;
}
if (reader->syntax == SERD_TURTLE) {
if (is_digit(n->buf[reader->bprefix_len + 1])) {
if ((n->buf[reader->bprefix_len]) == 'b') {
((char*)n->buf)[reader->bprefix_len] = 'B'; // Prevent clash
reader->seen_genid = true;
} else if (reader->seen_genid &&
n->buf[reader->bprefix_len] == 'B') {
r_err(reader, SERD_ERR_ID_CLASH,
"found both `b' and `B' blank IDs, prefix required\n");
return pop_node(reader, ref);
}
}
}
return ref;
}
static SerdStatus
read_utf8_character(SerdReader* reader, Ref dest, uint8_t c)
{
unsigned size = 1;
if ((c & 0xE0) == 0xC0) { // Starts with `110'
size = 2;
} else if ((c & 0xF0) == 0xE0) { // Starts with `1110'
size = 3;
} else if ((c & 0xF8) == 0xF0) { // Starts with `11110'
size = 4;
} else {
return bad_char(reader, dest, "invalid UTF-8 start 0x%X\n", c);
}
char bytes[4];
bytes[0] = c;
// Check character validity
for (unsigned i = 1; i < size; ++i) {
if (((bytes[i] = peek_byte(reader)) & 0x80) == 0) {
return bad_char(reader, dest, "invalid UTF-8 continuation 0x%X\n",
bytes[i]);
}
eat_byte_safe(reader, bytes[i]);
}
// Emit character
for (unsigned i = 0; i < size; ++i) {
push_byte(reader, dest, bytes[i]);
}
return SERD_SUCCESS;
}
static HSD_state st_yield_backref(heatshrink_decoder *hsd,
output_info *oi) {
size_t count = oi->buf_size - *oi->output_size;
if (count > 0) {
size_t i = 0;
if (hsd->output_count < count) count = hsd->output_count;
uint8_t *buf = &hsd->buffers[HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(hsd)];
uint16_t mask = (1 << HEATSHRINK_DECODER_WINDOW_BITS(hsd)) - 1;
uint16_t neg_offset = hsd->output_index;
LOG("-- emitting %zu bytes from -%u bytes back\n", count, neg_offset);
ASSERT(neg_offset <= mask + 1);
ASSERT(count <= (size_t)(1 << BACKREF_COUNT_BITS(hsd)));
for (i=0; i<count; i++) {
uint8_t c = buf[(hsd->head_index - neg_offset) & mask];
push_byte(hsd, oi, c);
buf[hsd->head_index & mask] = c;
hsd->head_index++;
LOG(" -- ++ 0x%02x\n", c);
}
hsd->output_count -= count;
if (hsd->output_count == 0) { return HSDS_TAG_BIT; }
}
return HSDS_YIELD_BACKREF;
}
// code should be a-z, A-Z or *
void gcode_add_word_int (tLineBuffer *pBuf, uint8_t code, int32_t data)
{
eArgType arg_type;
if (data < 0)
{
// signed
if ( (data < INT16_MIN))
arg_type = arg_int32;
else if ( (data < INT8_MIN))
arg_type = arg_int16;
else
arg_type = arg_int8;
}
else
{
if ( (data > UINT16_MAX))
arg_type = arg_int32;
else if ( data > UINT8_MAX)
arg_type = arg_uint16;
else
arg_type = arg_uint8;
}
code = toupper (code);
if ( (code >= 'A') && (code <= 'Z'))
code = ((code-64) << 3) | arg_type;
else
code = CODE_STAR;
push_byte (pBuf,code);
// must be at least one byte
push_byte (pBuf, data & 0xff);
data >>= 8;
if (arg_type >= arg_uint16)
{
push_byte (pBuf, data & 0xff);
data >>= 8;
}
static Ref
read_LANGTAG(SerdReader* reader)
{
uint8_t c = peek_byte(reader);
if (!is_alpha(c)) {
return r_err(reader, SERD_ERR_BAD_SYNTAX, "unexpected `%c'\n", c);
}
Ref ref = push_node(reader, SERD_LITERAL, "", 0);
push_byte(reader, ref, eat_byte_safe(reader, c));
while ((c = peek_byte(reader)) && is_alpha(c)) {
push_byte(reader, ref, eat_byte_safe(reader, c));
}
while (peek_byte(reader) == '-') {
push_byte(reader, ref, eat_byte_safe(reader, '-'));
while ((c = peek_byte(reader)) && (is_alpha(c) || is_digit(c))) {
push_byte(reader, ref, eat_byte_safe(reader, c));
}
}
return ref;
}
static bool
read_0_9(SerdReader* reader, Ref str, bool at_least_one)
{
unsigned count = 0;
for (uint8_t c; is_digit((c = peek_byte(reader))); ++count) {
push_byte(reader, str, eat_byte_safe(reader, c));
}
if (at_least_one && count == 0) {
r_err(reader, SERD_ERR_BAD_SYNTAX, "expected digit\n");
}
return count;
}
// Read ECHAR escape, initial \ is already eaten by caller
static inline bool
read_ECHAR(SerdReader* reader, Ref dest, SerdNodeFlags* flags)
{
const uint8_t c = peek_byte(reader);
switch (c) {
case 't':
eat_byte_safe(reader, 't');
push_byte(reader, dest, '\t');
return true;
case 'b':
eat_byte_safe(reader, 'b');
push_byte(reader, dest, '\b');
return true;
case 'n':
*flags |= SERD_HAS_NEWLINE;
eat_byte_safe(reader, 'n');
push_byte(reader, dest, '\n');
return true;
case 'r':
*flags |= SERD_HAS_NEWLINE;
eat_byte_safe(reader, 'r');
push_byte(reader, dest, '\r');
return true;
case 'f':
eat_byte_safe(reader, 'f');
push_byte(reader, dest, '\f');
return true;
case '\\': case '"': case '\'':
push_byte(reader, dest, eat_byte_safe(reader, c));
return true;
default:
return false;
}
}
static bool
read_PN_CHARS_BASE(SerdReader* reader, Ref dest)
{
const uint8_t c = peek_byte(reader);
if ((c & 0x80)) { // Multi-byte character
return !read_utf8_character(reader, dest, eat_byte_safe(reader, c));
}
if (is_alpha(c)) {
push_byte(reader, dest, eat_byte_safe(reader, c));
return true;
}
return false;
}
static SerdStatus
read_PN_LOCAL(SerdReader* reader, Ref dest, bool* ate_dot)
{
uint8_t c = peek_byte(reader);
SerdStatus st;
switch (c) {
case '0': case '1': case '2': case '3': case '4': case '5':
case '6': case '7': case '8': case '9': case ':': case '_':
push_byte(reader, dest, eat_byte_safe(reader, c));
break;
default:
if ((st = read_PLX(reader, dest)) > SERD_FAILURE) {
return st;
} else if (st != SERD_SUCCESS && !read_PN_CHARS_BASE(reader, dest)) {
return SERD_FAILURE;
}
}
while ((c = peek_byte(reader))) { // Middle: (PN_CHARS | '.' | ';')*
if (c == '.' || c == ':') {
push_byte(reader, dest, eat_byte_safe(reader, c));
} else if ((st = read_PLX(reader, dest)) > SERD_FAILURE) {
return st;
} else if (st != SERD_SUCCESS && !read_PN_CHARS(reader, dest)) {
break;
}
}
SerdNode* const n = deref(reader, dest);
if (n->buf[n->n_bytes - 1] == '.') {
// Ate trailing dot, pop it from stack/node and inform caller
--n->n_bytes;
serd_stack_pop(&reader->stack, 1);
*ate_dot = true;
}
return SERD_SUCCESS;
}
static bool
read_PrefixedName(SerdReader* reader, Ref dest, bool read_prefix, bool* ate_dot)
{
if (read_prefix) {
if (read_PN_PREFIX(reader, dest) > SERD_FAILURE) {
return false;
}
}
if (eat_byte_check(reader, ':') != ':') {
return false;
}
push_byte(reader, dest, ':');
return read_PN_LOCAL(reader, dest, ate_dot) <= SERD_FAILURE;
}
static void
read_stdin(int mode, struct pbc_slice *data) {
data->len = 128;
data->buffer = malloc(data->len);
int idx = 0;
while(!feof(stdin)) {
int byte;
int r = scanf("%d" , &byte);
if (r == 0) {
break;
}
push_byte(byte, data, idx);
++idx;
}
data->len = idx;
}
/***************************************************************
Return the current state of all sensors
***************************************************************/
void return_all_sensors()
{
//printf(" in return_all_sensors() ");
start_checksum();
push_char(0x50);
push_char(0xAF);
push_char(0x80);
// fake data for now
push_byte(get_sonar(1));
push_byte(get_sonar(2));
push_byte(get_sonar(3));
push_byte(get_line_follower(1));
push_byte(get_line_follower(2));
push_byte(get_line_follower(3));
push_byte(get_gyro());
push_byte(get_bumper(1));
push_byte(get_bumper(2));
send_checksum();
// Now 3 + 3 + 3 + 3 + 1 = 13
}
// Read one character (possibly multi-byte)
// The first byte, c, has already been eaten by caller
static inline SerdStatus
read_character(SerdReader* reader, Ref dest, SerdNodeFlags* flags, uint8_t c)
{
if (!(c & 0x80)) {
switch (c) {
case 0xA: case 0xD:
*flags |= SERD_HAS_NEWLINE;
break;
case '"': case '\'':
*flags |= SERD_HAS_QUOTE;
break;
}
push_byte(reader, dest, c);
return SERD_SUCCESS;
} else {
return read_utf8_character(reader, dest, c);
}
}
static HSD_state st_yield_literal(heatshrink_decoder *hsd,
output_info *oi) {
/* Emit a repeated section from the window buffer, and add it (again)
* to the window buffer. (Note that the repetition can include
* itself.)*/
if (*oi->output_size < oi->buf_size) {
uint32_t byte = get_bits(hsd, 8);
if (byte == NO_BITS) { return HSDS_YIELD_LITERAL; } /* out of input */
uint8_t *buf = &hsd->buffers[HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(hsd)];
uint16_t mask = (1 << HEATSHRINK_DECODER_WINDOW_BITS(hsd)) - 1;
uint8_t c = byte & 0xFF;
LOG("-- emitting literal byte 0x%02x ('%c')\n", c, isprint(c) ? c : '.');
buf[hsd->head_index++ & mask] = c;
push_byte(hsd, oi, c);
return HSDS_CHECK_FOR_MORE_INPUT;
} else {
return HSDS_YIELD_LITERAL;
}
}
// Read the remainder of a PN_PREFIX after some initial characters
static SerdStatus
read_PN_PREFIX_tail(SerdReader* reader, Ref dest)
{
uint8_t c;
while ((c = peek_byte(reader))) { // Middle: (PN_CHARS | '.')*
if (c == '.') {
push_byte(reader, dest, eat_byte_safe(reader, c));
} else if (!read_PN_CHARS(reader, dest)) {
break;
}
}
const SerdNode* const n = deref(reader, dest);
if (n->buf[n->n_bytes - 1] == '.' && !read_PN_CHARS(reader, dest)) {
r_err(reader, SERD_ERR_BAD_SYNTAX, "prefix ends with `.'\n");
return SERD_ERR_BAD_SYNTAX;
}
return SERD_SUCCESS;
}
// STRING_LITERAL_LONG_QUOTE and STRING_LITERAL_LONG_SINGLE_QUOTE
// Initial triple quotes are already eaten by caller
static Ref
read_STRING_LITERAL_LONG(SerdReader* reader, SerdNodeFlags* flags, uint8_t q)
{
Ref ref = push_node(reader, SERD_LITERAL, "", 0);
while (true) {
const uint8_t c = peek_byte(reader);
uint32_t code;
switch (c) {
case '\\':
eat_byte_safe(reader, c);
if (!read_ECHAR(reader, ref, flags) &&
!read_UCHAR(reader, ref, &code)) {
r_err(reader, SERD_ERR_BAD_SYNTAX,
"invalid escape `\\%c'\n", peek_byte(reader));
return pop_node(reader, ref);
}
break;
default:
if (c == q) {
eat_byte_safe(reader, q);
const uint8_t q2 = eat_byte_safe(reader, peek_byte(reader));
const uint8_t q3 = peek_byte(reader);
if (q2 == q && q3 == q) { // End of string
eat_byte_safe(reader, q3);
return ref;
} else {
*flags |= SERD_HAS_QUOTE;
push_byte(reader, ref, c);
read_character(reader, ref, flags, q2);
}
} else {
read_character(reader, ref, flags, eat_byte_safe(reader, c));
}
}
}
return ref;
}
static SerdStatus
read_PLX(SerdReader* reader, Ref dest)
{
uint8_t c = peek_byte(reader);
switch (c) {
case '%':
if (!read_PERCENT(reader, dest)) {
return SERD_ERR_BAD_SYNTAX;
}
return SERD_SUCCESS;
case '\\':
eat_byte_safe(reader, c);
if (is_alpha(c = peek_byte(reader))) {
// Escapes like \u \n etc. are not supported
return SERD_ERR_BAD_SYNTAX;
} else {
// Allow escaping of pretty much any other character
push_byte(reader, dest, eat_byte_safe(reader, c));
return SERD_SUCCESS;
}
default:
return SERD_FAILURE;
}
}
// Jumps to routine, given pointer to address in file header. Pushes idle_addr
// as return address, NOT old PC.
void Nsf_Impl::jsr_then_stop( byte const addr [] )
{
cpu.r.pc = get_addr( addr );
push_byte( (idle_addr - 1) >> 8 );
push_byte( (idle_addr - 1) );
}
void gcode_add_code (tLineBuffer *pBuf, uint8_t code)
{
push_byte (pBuf, code);
}
// Read UCHAR escape, initial \ is already eaten by caller
static inline bool
read_UCHAR(SerdReader* reader, Ref dest, uint32_t* char_code)
{
const uint8_t b = peek_byte(reader);
unsigned length = 0;
switch (b) {
case 'U':
length = 8;
break;
case 'u':
length = 4;
break;
default:
return false;
}
eat_byte_safe(reader, b);
uint8_t buf[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 0 };
for (unsigned i = 0; i < length; ++i) {
if (!(buf[i] = read_HEX(reader))) {
return false;
}
}
uint32_t code;
sscanf((const char*)buf, "%X", &code);
unsigned size = 0;
if (code < 0x00000080) {
size = 1;
} else if (code < 0x00000800) {
size = 2;
} else if (code < 0x00010000) {
size = 3;
} else if (code < 0x00110000) {
size = 4;
} else {
r_err(reader, SERD_ERR_BAD_SYNTAX,
"unicode character 0x%X out of range\n", code);
push_replacement(reader, dest);
*char_code = 0xFFFD;
return true;
}
// Build output in buf
// (Note # of bytes = # of leading 1 bits in first byte)
uint32_t c = code;
switch (size) {
case 4:
buf[3] = 0x80 | (uint8_t)(c & 0x3F);
c >>= 6;
c |= (16 << 12); // set bit 4
case 3:
buf[2] = 0x80 | (uint8_t)(c & 0x3F);
c >>= 6;
c |= (32 << 6); // set bit 5
case 2:
buf[1] = 0x80 | (uint8_t)(c & 0x3F);
c >>= 6;
c |= 0xC0; // set bits 6 and 7
case 1:
buf[0] = (uint8_t)c;
}
for (unsigned i = 0; i < size; ++i) {
push_byte(reader, dest, buf[i]);
}
*char_code = code;
return true;
}
static bool
read_number(SerdReader* reader, Ref* dest, Ref* datatype, bool* ate_dot)
{
#define XSD_DECIMAL NS_XSD "decimal"
#define XSD_DOUBLE NS_XSD "double"
#define XSD_INTEGER NS_XSD "integer"
Ref ref = push_node(reader, SERD_LITERAL, "", 0);
uint8_t c = peek_byte(reader);
bool has_decimal = false;
if (c == '-' || c == '+') {
push_byte(reader, ref, eat_byte_safe(reader, c));
}
if ((c = peek_byte(reader)) == '.') {
has_decimal = true;
// decimal case 2 (e.g. '.0' or `-.0' or `+.0')
push_byte(reader, ref, eat_byte_safe(reader, c));
TRY_THROW(read_0_9(reader, ref, true));
} else {
// all other cases ::= ( '-' | '+' ) [0-9]+ ( . )? ( [0-9]+ )? ...
TRY_THROW(is_digit(c));
read_0_9(reader, ref, true);
if ((c = peek_byte(reader)) == '.') {
has_decimal = true;
// Annoyingly, dot can be end of statement, so tentatively eat
eat_byte_safe(reader, c);
c = peek_byte(reader);
if (!is_digit(c) && c != 'e' && c != 'E') {
*dest = ref;
*ate_dot = true; // Force caller to deal with stupid grammar
return true; // Next byte is not a number character, done
}
push_byte(reader, ref, '.');
read_0_9(reader, ref, false);
}
}
c = peek_byte(reader);
if (c == 'e' || c == 'E') {
// double
push_byte(reader, ref, eat_byte_safe(reader, c));
switch ((c = peek_byte(reader))) {
case '+': case '-':
push_byte(reader, ref, eat_byte_safe(reader, c));
default: break;
}
TRY_THROW(read_0_9(reader, ref, true));
*datatype = push_node(reader, SERD_URI,
XSD_DOUBLE, sizeof(XSD_DOUBLE) - 1);
} else if (has_decimal) {
*datatype = push_node(reader, SERD_URI,
XSD_DECIMAL, sizeof(XSD_DECIMAL) - 1);
} else {
*datatype = push_node(reader, SERD_URI,
XSD_INTEGER, sizeof(XSD_INTEGER) - 1);
}
*dest = ref;
return true;
except:
pop_node(reader, *datatype);
pop_node(reader, ref);
return false;
}
static void push_firq_registers(struct MC6809 *cpu) {
NVMA_CYCLE;
push_word(cpu, ®_S, REG_PC);
push_byte(cpu, ®_S, REG_CC);
}
