Commit 118fb412 authored by Sven Gestegård Robertz's avatar Sven Gestegård Robertz
Browse files

copied c lib and started working on version 2006

parent 72af701b
// C Preprocessor macros.
#ifndef CPP_MACROS_H
#define CPP_MACROS_H
#define X_EMPTY(mac) var ## 1
#define EMPTY(mac) X_EMPTY(mac) // Returns 1 if macro mac is empty.
#endif
/*
labcomm.c -- runtime for handling encoding and decoding of
labcomm samples.
Copyright 2006-2013 Anders Blomdell <anders.blomdell@control.lth.se>
This file is part of LabComm.
LabComm is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
LabComm is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifdef LABCOMM_COMPAT
#include LABCOMM_COMPAT
#else
#include <stdio.h>
#include <strings.h>
#endif
#include <errno.h>
#include <string.h>
#include <stdarg.h>
#include <stddef.h>
#include "labcomm.h"
#include "labcomm_private.h"
#include "labcomm_ioctl.h"
#include "labcomm_dynamic_buffer_writer.h"
#define LABCOMM_VERSION "LabComm2006"
/* Unwrapping reader/writer functions */
#define UNWRAP_ac(rw, ac, ...) ac
#define UNWRAP(func, ...) \
while (1) { \
if (UNWRAP_ac(__VA_ARGS__)->action->func) { \
return UNWRAP_ac(__VA_ARGS__)->action->func(__VA_ARGS__); } \
if (UNWRAP_ac(__VA_ARGS__)->next == NULL) { return -ENOSYS; } \
UNWRAP_ac( __VA_ARGS__) = UNWRAP_ac(__VA_ARGS__)->next; \
}
int labcomm_reader_alloc(struct labcomm_reader *r,
struct labcomm_reader_action_context *action_context,
char *labcomm_version)
{
UNWRAP(alloc, r, action_context, labcomm_version);
}
int labcomm_reader_free(struct labcomm_reader *r,
struct labcomm_reader_action_context *action_context)
{
UNWRAP(free, r, action_context);
}
int labcomm_reader_start(struct labcomm_reader *r,
struct labcomm_reader_action_context *action_context,
int local_index, int remote_index,
struct labcomm_signature *signature,
void *value)
{
UNWRAP(start, r, action_context, local_index, remote_index, signature, value);
}
int labcomm_reader_end(struct labcomm_reader *r,
struct labcomm_reader_action_context *action_context)
{
UNWRAP(end, r, action_context);
}
int labcomm_reader_fill(struct labcomm_reader *r,
struct labcomm_reader_action_context *action_context)
{
UNWRAP(fill, r, action_context);
}
int labcomm_reader_ioctl(struct labcomm_reader *r,
struct labcomm_reader_action_context *action_context,
int local_index, int remote_index,
struct labcomm_signature *signature,
uint32_t ioctl_action, va_list args)
{
UNWRAP(ioctl, r, action_context,
local_index, remote_index, signature, ioctl_action, args);
}
int labcomm_writer_alloc(struct labcomm_writer *w,
struct labcomm_writer_action_context *action_context,
char *labcomm_version)
{
UNWRAP(alloc, w, action_context, labcomm_version);
}
int labcomm_writer_free(struct labcomm_writer *w,
struct labcomm_writer_action_context *action_context)
{
UNWRAP(free, w, action_context);
}
int labcomm_writer_start(struct labcomm_writer *w,
struct labcomm_writer_action_context *action_context,
int index, struct labcomm_signature *signature,
void *value)
{
UNWRAP(start, w, action_context, index, signature, value);
}
int labcomm_writer_end(struct labcomm_writer *w,
struct labcomm_writer_action_context *action_context)
{
UNWRAP(end, w, action_context);
}
int labcomm_writer_flush(struct labcomm_writer *w,
struct labcomm_writer_action_context *action_context)
{
UNWRAP(flush, w, action_context);
}
int labcomm_writer_ioctl(struct labcomm_writer *w,
struct labcomm_writer_action_context *action_context,
int index,
struct labcomm_signature *signature,
uint32_t ioctl_action, va_list args)
{
UNWRAP(ioctl, w, action_context, index, signature, ioctl_action, args);
}
#undef UNWRAP
#undef UNWRAP_ac
static const char *labcomm_error_string[] = {
#define LABCOMM_ERROR(name, description) description ,
#include "labcomm_error.h"
#undef LABCOMM_ERROR
};
static const int labcomm_error_string_count = (sizeof(labcomm_error_string) /
sizeof(labcomm_error_string[0]));
const char *labcomm_error_get_str(enum labcomm_error error_id)
{
const char *error_str = NULL;
// Check if this is a known error ID.
if (error_id < labcomm_error_string_count) {
error_str = labcomm_error_string[error_id];
}
return error_str;
}
void on_error_fprintf(enum labcomm_error error_id, size_t nbr_va_args, ...)
{
#ifndef LABCOMM_NO_STDIO
const char *err_msg = labcomm_error_get_str(error_id); // The final string to print.
if (err_msg == NULL) {
err_msg = "Error with an unknown error ID occured.";
}
fprintf(stderr, "%s\n", err_msg);
if (nbr_va_args > 0) {
va_list arg_pointer;
va_start(arg_pointer, nbr_va_args);
fprintf(stderr, "%s\n", "Extra info {");
char *print_format = va_arg(arg_pointer, char *);
vfprintf(stderr, print_format, arg_pointer);
fprintf(stderr, "}\n");
va_end(arg_pointer);
}
#else
; // If labcomm can't be compiled with stdio the user will have to make an own error callback functionif he/she needs error reporting.
#endif
}
#if 0
static void dump(void *p, int size, int first, int last)
{
int i, j;
printf("%d %d (%p): ", first, last, p);
for (i = first ; i < last ; i++) {
for (j = 0 ; j < size ; j++) {
printf("%2.2d", ((char*)p)[(i-first)*size + j]);
}
printf(" ");
}
printf("\n");
}
#endif
void *labcomm_signature_array_ref(struct labcomm_memory *memory,
int *first, int *last, void **data,
int size, int index)
{
if (*first == 0 && *last == 0) {
*first = index;
*last = index + 1;
*data = labcomm_memory_alloc(memory, 0, size);
if (*data) {
memset(*data, 0, size);
}
} else if (index < *first || *last <= index) {
void *old_data = *data;
int old_first = *first;
int old_last = *last;
int n;
*first = (index<old_first)?index:old_first;
*last = (old_last<=index)?index+1:old_last;
n = (*last - *first);
*data = labcomm_memory_alloc(memory, 0, n * size);
if (*data) {
memset(*data, 0, n * size);
memcpy(*data + (old_first - *first) * size,
old_data,
(old_last - old_first) * size);
}
// dump(old_data, size, old_first, old_last);
labcomm_memory_free(memory, 0, old_data);
}
if (*data) {
// dump(*data, size, *first, *last);
return *data + (index - *first) * size;
} else {
return NULL;
}
}
static int local_index = 0x40;
void labcomm_set_local_index(struct labcomm_signature *signature)
{
if (signature->index != 0) {
labcomm_error_fatal_global(LABCOMM_ERROR_SIGNATURE_ALREADY_SET,
"%s", signature->name);
}
signature->index = local_index;
local_index++;
}
int labcomm_get_local_index(struct labcomm_signature *signature)
{
if (signature->index == 0) {
labcomm_error_fatal_global(LABCOMM_ERROR_SIGNATURE_NOT_SET,
"%s", signature->name);
}
return signature->index;
}
/*
labcomm.h -- user interface for handling encoding and decoding of
labcomm samples.
Copyright 2006-2013 Anders Blomdell <anders.blomdell@control.lth.se>
This file is part of LabComm.
LabComm is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
LabComm is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef _LABCOMM_H_
#define _LABCOMM_H_
#include <stdarg.h>
#include <stdint.h>
#include <unistd.h>
#include "labcomm_error.h"
#include "labcomm_scheduler.h"
/* Forward declaration */
struct labcomm_encoder;
struct labcomm_decoder;
/*
* Signature entry
*/
struct labcomm_signature {
int type;
char *name;
int (*encoded_size)(struct labcomm_signature *, void *); // void * == encoded_sample *
int size;
unsigned char *signature;
int index;
#ifdef LABCOMM_EXPERIMENTAL_CACHED_ENCODED_SIZE
int cached_encoded_size; // -1 if not initialized or type is variable size
#endif
};
/*
* Error handling.
*/
/* The callback prototype for error handling.
* First parameter is the error ID.
* The second paramters is the number of va_args that comes after this
* one. If none it should be 0.
* Optionaly other paramters can be supplied depending on what is needed
* for this error ID.
*/
typedef void (*labcomm_error_handler_callback)(enum labcomm_error error_id,
size_t nbr_va_args, ...);
/* Default error handler, prints message to stderr.
* Extra info about the error can be supplied as char* as VA-args. Especially user defined errors should supply a describing string. if nbr_va_args > 1 the first variable argument must be a printf format string and the possibly following arguments are passed as va_args to vprintf.
*/
void on_error_fprintf(enum labcomm_error error_id, size_t nbr_va_args, ...);
/* Register a callback for the error handler for this encoder. */
void labcomm_register_error_handler_encoder(struct labcomm_encoder *encoder, labcomm_error_handler_callback callback);
/* Register a callback for the error handler for this decoder. */
void labcomm_register_error_handler_decoder(struct labcomm_decoder *decoder, labcomm_error_handler_callback callback);
/* Get a string describing the supplied standrad labcomm error. */
const char *labcomm_error_get_str(enum labcomm_error error_id);
typedef int (*labcomm_handle_new_datatype_callback)(
struct labcomm_decoder *decoder,
struct labcomm_signature *sig);
void labcomm_decoder_register_new_datatype_handler(struct labcomm_decoder *d,
labcomm_handle_new_datatype_callback on_new_datatype);
/*
* Dynamic memory handling
* lifetime == 0 memory that will live for as long as the
* encoder/decoder or that are allocated/deallocated
* during the communication setup phase
* otherwise memory will live for approximately this number of
* sent/received samples
*/
struct labcomm_memory;
void *labcomm_memory_alloc(struct labcomm_memory *m, int lifetime, size_t size);
void *labcomm_memory_realloc(struct labcomm_memory *m, int lifetime,
void *ptr, size_t size);
void labcomm_memory_free(struct labcomm_memory *m, int lifetime, void *ptr);
/*
* Decoder
*/
struct labcomm_reader;
struct labcomm_decoder *labcomm_decoder_new(
struct labcomm_reader *reader,
struct labcomm_error_handler *error,
struct labcomm_memory *memory,
struct labcomm_scheduler *scheduler);
void labcomm_decoder_free(
struct labcomm_decoder *decoder);
int labcomm_decoder_decode_one(
struct labcomm_decoder *decoder);
void labcomm_decoder_run(
struct labcomm_decoder *decoder);
/* See labcomm_ioctl.h for predefined ioctl_action values */
int labcomm_decoder_ioctl(struct labcomm_decoder *decoder,
uint32_t ioctl_action,
...);
/*
* Encoder
*/
struct labcomm_writer;
struct labcomm_encoder *labcomm_encoder_new(
struct labcomm_writer *writer,
struct labcomm_error_handler *error,
struct labcomm_memory *memory,
struct labcomm_scheduler *scheduler);
void labcomm_encoder_free(
struct labcomm_encoder *encoder);
/* See labcomm_ioctl.h for predefined ioctl_action values */
int labcomm_encoder_ioctl(struct labcomm_encoder *encoder,
uint32_t ioctl_action,
...);
#define LABCOMM_VOID ((void*)1)
#endif
#ifndef ARM_CORTEXM3_CODESOURCERY
#error "ARM_CORTEXM3_CODESOURCERY" not defined
#endif
#include <machine/endian.h>
#ifndef __APPLE__
#error "__APPLE__" not defined
#endif
#ifndef LABCOMM_COMPAT_OSX
#define LABCOMM_COMPAT_OSX
#include <machine/endian.h>
#include <stdio.h>
#include <time.h>
#include <mach/clock.h>
#include <mach/mach.h>
#define CLOCK_REALTIME 0
static inline void clock_gettime(int garbage, struct timespec *ts)
{
(void) garbage;
clock_serv_t cclock;
mach_timespec_t mts;
host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock);
clock_get_time(cclock, &mts);
mach_port_deallocate(mach_task_self(), cclock);
ts->tv_sec = mts.tv_sec;
ts->tv_nsec = mts.tv_nsec;
}
#endif
#ifndef __VXWORKS__
#error "__VXWORKS__" not defined
#endif
#if (CPU == PPC603)
#undef _LITTLE_ENDIAN
#endif
#if (CPU == PENTIUM4)
#undef _BIG_ENDIAN
#endif
/*
labcomm_decoder.c -- runtime for handling decoding of labcomm samples.
Copyright 2006-2013 Anders Blomdell <anders.blomdell@control.lth.se>
This file is part of LabComm.
LabComm is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
LabComm is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#define LABCOMM_VERSION "LabComm2006"
#include <errno.h>
#include "labcomm.h"
#include "labcomm_private.h"
#include "labcomm_ioctl.h"
#include "labcomm_dynamic_buffer_writer.h"
struct sample_entry {
int remote_index;
struct labcomm_signature *signature;
labcomm_decoder_function decode;
labcomm_handler_function handler;
void *context;
};
struct labcomm_decoder {
struct labcomm_reader *reader;
int reader_allocated;
struct labcomm_error_handler *error;
struct labcomm_memory *memory;
struct labcomm_scheduler *scheduler;
labcomm_error_handler_callback on_error;
labcomm_handle_new_datatype_callback on_new_datatype;
LABCOMM_SIGNATURE_ARRAY_DEF(local, struct sample_entry);
LABCOMM_SIGNATURE_ARRAY_DEF(remote_to_local, int);
};
struct labcomm_decoder *labcomm_decoder_new(
struct labcomm_reader *reader,
struct labcomm_error_handler *error,
struct labcomm_memory *memory,
struct labcomm_scheduler *scheduler)
{
struct labcomm_decoder *result;
result = labcomm_memory_alloc(memory, 0, sizeof(*result));
if (result) {
result->reader = reader;
result->reader->decoder = result;
result->reader->data = 0;
result->reader->data_size = 0;
result->reader->count = 0;
result->reader->pos = 0;
result->reader->error = 0;
result->reader_allocated = 0;
result->error = error;
result->memory = memory;
result->scheduler = scheduler;
LABCOMM_SIGNATURE_ARRAY_INIT(result->local, struct sample_entry);
LABCOMM_SIGNATURE_ARRAY_INIT(result->remote_to_local, int);
}
return result;
}
void labcomm_decoder_free(struct labcomm_decoder* d)
{
struct labcomm_memory *memory = d->memory;
labcomm_reader_free(d->reader, d->reader->action_context);
LABCOMM_SIGNATURE_ARRAY_FREE(memory, d->local, struct sample_entry);
LABCOMM_SIGNATURE_ARRAY_FREE(memory, d->remote_to_local, int);
labcomm_memory_free(memory, 0, d);
}
static int collect_flat_signature(
struct labcomm_decoder *decoder,
struct labcomm_writer *writer)
{
int result, type;
type = labcomm_read_packed32(decoder->reader);
result = decoder->reader->error;
if (result < 0) { goto out; }
if (type >= LABCOMM_USER) {
decoder->on_error(LABCOMM_ERROR_UNIMPLEMENTED_FUNC, 3,
"Implement %s ... (1) for type 0x%x\n", __FUNCTION__, type);
} else {
labcomm_write_packed32(writer, type);
switch (type) {
case LABCOMM_ARRAY: {
int dimensions, i;
dimensions = labcomm_read_packed32(decoder->reader);
labcomm_write_packed32(writer, dimensions);
for (i = 0 ; i < dimensions ; i++) {
int n = labcomm_read_packed32(decoder->reader);
labcomm_write_packed32(writer, n);
}
result = collect_flat_signature(decoder, writer);
if (result < 0) { goto out; }
} break;
case LABCOMM_STRUCT: {
int fields, i;
fields = labcomm_read_packed32(decoder->reader);
labcomm_write_packed32(writer, fields);
for (i = 0 ; i < fields ; i++) {
char *name = labcomm_read_string(decoder->reader);
labcomm_write_string(writer, name);
labcomm_memory_free(decoder->memory, 1, name);
result = collect_flat_signature(decoder, writer);
if (result < 0) { goto out; }
}
} break;
case LABCOMM_BOOLEAN:
case LABCOMM_BYTE:
case LABCOMM_SHORT:
case LABCOMM_INT:
case LABCOMM_LONG:
case LABCOMM_FLOAT:
case LABCOMM_DOUBLE:
case LABCOMM_STRING: {
} break;
default: {
result = -ENOSYS;
decoder->on_error(LABCOMM_ERROR_UNIMPLEMENTED_FUNC, 3,
"Implement %s (2) for type 0x%x...\n", __FUNCTION__, type);
} break;
}
}
out:
return result;
}
static int writer_ioctl(struct labcomm_writer *writer,
uint32_t action,
...)
{
int result;
va_list va;
if (LABCOMM_IOC_SIG(action) != LABCOMM_IOC_NOSIG) {
result = -EINVAL;
goto out;
}