size_t read_terminal_seq(struct terminal_input *inp)
{
char *buf = inp->next.s;
size_t len = get_seq_len(buf, inp->next_length);
if (len == 0) {
/* make sure buffer is not full before reading */
inp->next_length = truncate_seq(buf, inp->next_length, 8);
len = read_into_buffer(inp, inp->next_length);
if (len > 0) {
inp->next_length += len;
len = get_seq_len(buf, inp->next_length);
}
}
if (len > 0) {
inp->current = inp->next;
if (len < inp->next_length) {
inp->next_length -= len;
} else {
inp->next_length = 0;
}
/* move remaining input to next buffer */
memcpy(buf, inp->current.s + len, inp->next_length + 1);
}
inp->current.s[len] = '\0';
return len;
}
int
main (int argc, char **argv)
{
gpg_error_t err;
if (argc)
{
argc--;
argv++;
}
if (!argc)
{
test_oid_to_str ();
}
else if (!strcmp (*argv, "--from-str"))
{
unsigned char *buffer;
size_t n, buflen;
for (argv++,argc-- ; argc; argc--, argv++)
{
err = ksba_oid_from_str (*argv, &buffer, &buflen);
if (err)
{
fprintf (stderr, "can't convert `%s': %s\n",
*argv, gpg_strerror (err));
return 1;
}
printf ("%s ->", *argv);
for (n=0; n < buflen; n++)
printf (" %02X", buffer[n]);
putchar ('\n');
free (buffer);
}
}
else if (!strcmp (*argv, "--to-str"))
{
char *buffer;
size_t buflen;
char *result;
argv++;argc--;
buffer = read_into_buffer (stdin, &buflen);
result = ksba_oid_to_str (buffer, buflen);
free (buffer);
printf ("%s\n", result? result:"[malloc failed]");
free (result);
}
else
{
fputs ("usage: "PGM" [--from-str|--to-str]\n", stderr);
return 1;
}
return 0;
}
/**
* Opens a new scanner on the specified file descriptor and allocates some internal resources.
*/
scanner_p scan_open(int fd){
scanner_p scanner = malloc(sizeof(scanner_t));
*scanner = (scanner_t){
.fd = fd, .buffer_ptr = malloc(4096),
.buffer_size = 4096, .buffer_pos = 0, .buffer_consumed = 0, .buffer_filled = 0,
.line = 1, .col = 1, .eof = false
};
return scanner;
}
/**
* Opens a new scanner with the specified string as buffer content. The scanner will encounter
* an EOF at the end of the string.
*/
scanner_p scan_open_string(char *code){
size_t len = strlen(code);
scanner_p scanner = malloc(sizeof(scanner_t));
*scanner = (scanner_t){
.fd = -1, .buffer_ptr = code,
.buffer_size = len, .buffer_pos = 0, .buffer_consumed = 0, .buffer_filled = len,
.line = 1, .col = 1, .eof = false
};
return scanner;
}
/**
* Closes the scanner and frees all associated resources.
*/
void scan_close(scanner_p scanner){
if (scanner->fd != -1)
free(scanner->buffer_ptr);
free(scanner);
}
/**
* Fills the buffer with available input from the file descriptor or blocks waiting for it. Returns
* whatever read() returned and leaves errno unchanged (so you can check yourself). If the
* scanner has no associated file descriptor 0 is returned (read's code for EOF).
*/
static int read_into_buffer(scanner_p scanner){
if (scanner->fd == -1)
return 0;
// First move all buffer contents to the front of the buffer. We no longer need the consumed part of the buffer.
if (scanner->buffer_consumed > 0){
memmove(scanner->buffer_ptr, scanner->buffer_ptr + scanner->buffer_consumed, scanner->buffer_filled - scanner->buffer_consumed);
scanner->buffer_pos -= scanner->buffer_consumed;
scanner->buffer_filled -= scanner->buffer_consumed;
scanner->buffer_consumed = 0;
}
// Then check if the buffer is still full. If it is expand it.
if ( scanner->buffer_filled == scanner->buffer_size ){
scanner->buffer_size += 4096;
scanner->buffer_ptr = realloc(scanner->buffer_ptr, scanner->buffer_size);
}
ssize_t bytes_read = read(scanner->fd, scanner->buffer_ptr + scanner->buffer_filled, scanner->buffer_size - scanner->buffer_filled);
if (bytes_read > 0)
scanner->buffer_filled += bytes_read;
return bytes_read;
}
/**
* Returns the next character in the stream without consuming it.
*/
int scan_peek(scanner_p scanner){
if (scanner->eof)
return EOF;
if (scanner->buffer_pos >= scanner->buffer_filled){
ssize_t bytes_read = read_into_buffer(scanner);
if (bytes_read == 0){
scanner->eof = true;
return EOF;
}
}
return scanner->buffer_ptr[scanner->buffer_pos];
}
/**
* Returns the next character from the scanner buffer or EOF at the end of the file. If we're at the end of the scanner
* buffer new data is read from the file descriptor.
*
* This function does not mark the character as consumed! You have to increment the buffer_consumed member of the
* scanner yourself.
*/
static int read_next_char(scanner_p scanner){
int c = scan_peek(scanner);
if (c == EOF)
return EOF;
scanner->buffer_pos++;
if (c == '\n') {
scanner->line++;
scanner->prev_col = scanner->col;
scanner->col = 1;
} else {
scanner->col++;
}
return c;
}
char * buffered_reader::read_line(void)
{
char *out = NULL;
#ifdef USE_MMAP
if (mmap_addr)
{
long long int n_bytes;
char *lf;
char *virtual_0x00 = &mmap_addr[size_of_file];
/* EOF reached? */
if (!cur_offset)
return NULL;
/* determine length of current line */
lf = (char *)memchr(cur_offset, '\n', (virtual_0x00 - cur_offset));
if (lf)
n_bytes = lf - cur_offset;
else
n_bytes = virtual_0x00 - cur_offset;
/* allocate memory & copy string */
out = (char *)malloc(n_bytes + 1);
if (!out)
{
fprintf(stderr, "buffered_reader::read_line: malloc(%lld) failed\n", n_bytes + 1);
syslog(LOG_EMERG, "buffered_reader::read_line: malloc(%lld) failed", n_bytes + 1);
exit(1);
}
memcpy(out, cur_offset, n_bytes);
out[n_bytes] = 0x00;
if (lf)
cur_offset = lf + 1;
else
cur_offset = NULL;
}
else
#endif
{
long long int lf_offset = -1;
long long int n_bytes, search_start;
if (number_of_bytes_in_buffer() <= 0)
{
garbage_collect();
if (read_into_buffer() == 0)
{
// EOF
return NULL;
}
}
search_start = buffer_pointer;
for(;;)
{
char *dummy = strchr(&buffer[buffer_pointer], '\n');
if (dummy)
lf_offset = (long long int)(dummy - buffer);
if (lf_offset != -1)
break;
if (read_into_buffer() == 0)
{
lf_offset = buffer_length;
break;
}
}
n_bytes = lf_offset - buffer_pointer;
out = strndup(&buffer[buffer_pointer], n_bytes);
if (!out)
{
fprintf(stderr, "buffered_reader::read_line: malloc(%lld) failed\n", n_bytes + 1);
syslog(LOG_EMERG, "buffered_reader::read_line: malloc(%lld) failed", n_bytes + 1);
exit(1);
}
buffer_pointer = lf_offset + 1;
}
return out;
}
static int get_msg(struct _crrcsim *io, byte *data_buffer)
{
struct inputbuf *c = &io->buf;
int count = 0;
int packet_len;
int i;
read_into_buffer(io);
while (1) {
/*********************************************************************
* Find start of packet: the header (2 bytes) starts with 0x5555
*********************************************************************/
while (c->length >= 4 && (c->buf[c->start] != (byte)0x55 || c->buf[(byte)(c->start + 1)] != (byte)0x55)) {
c->start++;
c->length--;
}
if (c->length < 4) {
return count;
}
/*********************************************************************
* Read packet contents
*********************************************************************/
packet_len = 0;
switch (c->buf[(byte)(c->start + 2)]) {
case 'S':
packet_len = IMU_PACKET_LENGTH;
break;
case 'N':
packet_len = GPS_PACKET_LENGTH;
break;
case 'I':
packet_len = AHRS_PACKET_LENGTH;
break;
default:
break;
}
if (packet_len > 0 && c->length < packet_len) {
return count; // not enough data
}
if (packet_len > 0) {
byte ib;
word rcvchecksum = 0;
word sum = 0;
for (i = 2, ib = c->start + (byte)2; i < packet_len - 2; i++, ib++) {
sum += c->buf[ib];
}
rcvchecksum = c->buf[ib++] << 8;
rcvchecksum = rcvchecksum | c->buf[ib++];
if (rcvchecksum != sum) {
packet_len = 0;
printf("checksum error\n");
}
}
// fill data buffer or go to next bytes
if (packet_len > 0) {
for (i = 0; i < packet_len; i++) {
data_buffer[i] = c->buf[c->start];
c->start++;
c->length--;
}
count++;
} else {
c->start += 3;
c->length -= 3;
}
}
return count;
}
