labcomm.c 11.9 KB
Newer Older
Anders Nilsson's avatar
Anders Nilsson committed
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
#include <errno.h>
#include <string.h>
#include <stdio.h>
#ifndef __VXWORKS__
#include <strings.h>
#endif
#include <stdlib.h>
#ifdef __VXWORKS__
#if (CPU == PPC603)
#undef _LITTLE_ENDIAN
#endif
#if (CPU == PENTIUM4)
#undef _BIG_ENDIAN
#endif
#endif
#include "labcomm.h"
#include "labcomm_private.h"

typedef struct labcomm_sample_entry {
  struct labcomm_sample_entry *next;
  int index;
  labcomm_signature_t *signature;
  labcomm_decoder_typecast_t decoder;
  labcomm_handler_typecast_t handler;
  labcomm_encode_typecast_t encode;
  void *context;
} labcomm_sample_entry_t;

typedef struct labcomm_encoder_context {
  labcomm_sample_entry_t *sample;
  int index;
} labcomm_encoder_context_t;

typedef struct labcomm_decoder_context {
  labcomm_sample_entry_t *sample;
} labcomm_decoder_context_t;

static labcomm_sample_entry_t *get_sample_by_signature_address(
  labcomm_sample_entry_t *head,
  labcomm_signature_t *signature)
{
  labcomm_sample_entry_t *p;
  for (p = head ; p && p->signature != signature ; p = p->next) {
Riccardo Gaiati's avatar
Riccardo Gaiati committed
44

Anders Nilsson's avatar
Anders Nilsson committed
45
46
47
48
49
50
51
52
53
54
55
56
57
  }
  return p;
}

static labcomm_sample_entry_t *get_sample_by_signature_value(
  labcomm_sample_entry_t *head,
  labcomm_signature_t *signature)
{
  labcomm_sample_entry_t *p;
  for (p = head ; p ; p = p->next) {
    if (p->signature->type == signature->type &&
	p->signature->size == signature->size &&
	strcmp(p->signature->name, signature->name) == 0 &&
Riccardo Gaiati's avatar
Riccardo Gaiati committed
58
	bcmp((void*)p->signature->signature, (void*)signature->signature,
Anders Nilsson's avatar
Anders Nilsson committed
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
	     signature->size) == 0) {
      break;
    }
  }
  return p;
}

static labcomm_sample_entry_t *get_sample_by_index(
  labcomm_sample_entry_t *head,
  int index)
{
  labcomm_sample_entry_t *p;
  for (p = head ; p && p->index != index ; p = p->next) {
  }
  return p;
}

static int get_encoder_index(
  labcomm_encoder_t *e,
  labcomm_signature_t *s)
{
  int result = 0;
  labcomm_encoder_context_t *context = e->context;
  labcomm_sample_entry_t *sample = context->sample;
  while (sample) {
    if (sample->signature == s) { break; }
    sample = sample->next;
  }
  if (sample) {
    result = sample->index;
  }
  return result;
}

Riccardo Gaiati's avatar
Riccardo Gaiati committed
93
static void do_encoder_register(struct labcomm_encoder *e,
Anders Nilsson's avatar
Anders Nilsson committed
94
95
96
97
98
99
100
				labcomm_signature_t *signature,
				labcomm_encode_typecast_t encode)
{
  if (signature->type == LABCOMM_SAMPLE) {
    if (get_encoder_index(e, signature) == 0) {
      int i;
      labcomm_encoder_context_t *context = e->context;
Riccardo Gaiati's avatar
Riccardo Gaiati committed
101
      labcomm_sample_entry_t *sample =
Anders Nilsson's avatar
Anders Nilsson committed
102
103
104
105
106
107
108
109
110
	(labcomm_sample_entry_t*)malloc(sizeof(labcomm_sample_entry_t));
      sample->next = context->sample;
      sample->index = context->index;
      sample->signature = signature;
      sample->encode = encode;
      context->index++;
      context->sample = sample;

      e->writer.write(&e->writer, labcomm_writer_start);
111
      labcomm_encode_packed32(e, signature->type);
Anders Nilsson's avatar
Anders Nilsson committed
112
113
114
      labcomm_encode_type_index(e, signature);
      labcomm_encode_string(e, signature->name);
      for (i = 0 ; i < signature->size ; i++) {
Riccardo Gaiati's avatar
Riccardo Gaiati committed
115
	if (e->writer.pos >= e->writer.count) {
Anders Nilsson's avatar
Anders Nilsson committed
116
117
118
119
120
121
122
123
124
125
126
	  e->writer.write(&e->writer, labcomm_writer_continue);
	}
	e->writer.data[e->writer.pos] = signature->signature[i];
	e->writer.pos++;
      }
      e->writer.write(&e->writer, labcomm_writer_end);
    }
  }
}

static void do_encode(
Riccardo Gaiati's avatar
Riccardo Gaiati committed
127
128
  labcomm_encoder_t *encoder,
  labcomm_signature_t *signature,
Anders Nilsson's avatar
Anders Nilsson committed
129
130
131
132
  void *value)
{
  labcomm_encoder_context_t *context = encoder->context;
  labcomm_sample_entry_t *sample;
Riccardo Gaiati's avatar
Riccardo Gaiati committed
133
  sample = get_sample_by_signature_address(context->sample,
Anders Nilsson's avatar
Anders Nilsson committed
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
					   signature);
  if (sample && sample->encode) {
    sample->encode(encoder, value);
  } else {
    printf("Encoder has no registration for %s\n", signature->name);
  }
}

labcomm_encoder_t *labcomm_encoder_new(
  int (*writer)(labcomm_writer_t *, labcomm_writer_action_t),
  void *writer_context)
{
  labcomm_encoder_t *result = malloc(sizeof(labcomm_encoder_t));
  if (result) {
    labcomm_encoder_context_t *context;

    context = malloc(sizeof(labcomm_encoder_context_t));
    context->sample = 0;
    context->index = LABCOMM_USER;
    result->context = context;
    result->writer.context = writer_context;
    result->writer.data = 0;
    result->writer.data_size = 0;
    result->writer.count = 0;
    result->writer.pos = 0;
    result->writer.write = writer;
    result->writer.write(&result->writer, labcomm_writer_alloc);
    result->do_register = do_encoder_register;
    result->do_encode = do_encode;
  }
  return result;
}

void labcomm_internal_encoder_register(
Riccardo Gaiati's avatar
Riccardo Gaiati committed
168
  labcomm_encoder_t *e,
Anders Nilsson's avatar
Anders Nilsson committed
169
170
171
172
173
174
175
176
177
178
179
  labcomm_signature_t *signature,
  labcomm_encode_typecast_t encode)
{
  if (e && e->do_register) {
    e->do_register(e, signature, encode);
  } else {
    printf("Encoder is missing do_register\n");
  }
}

void labcomm_internal_encode(
Riccardo Gaiati's avatar
Riccardo Gaiati committed
180
181
  labcomm_encoder_t *e,
  labcomm_signature_t *signature,
Anders Nilsson's avatar
Anders Nilsson committed
182
183
184
185
186
187
  void *value)
{
  if (e && e->do_encode) {
    e->do_encode(e, signature, value);
  } else {
    printf("Encoder is missing do_encode\n");
Riccardo Gaiati's avatar
Riccardo Gaiati committed
188
  }
Anders Nilsson's avatar
Anders Nilsson committed
189
}
Riccardo Gaiati's avatar
Riccardo Gaiati committed
190

Anders Nilsson's avatar
Anders Nilsson committed
191
192
193
194
195
196
void labcomm_internal_encoder_user_action(labcomm_encoder_t *e,
					  int action)
{
  e->writer.write(&e->writer, action);
}

Riccardo Gaiati's avatar
Riccardo Gaiati committed
197
void labcomm_encoder_free(labcomm_encoder_t* e)
Anders Nilsson's avatar
Anders Nilsson committed
198
{
Riccardo Gaiati's avatar
Riccardo Gaiati committed
199

Anders Nilsson's avatar
Anders Nilsson committed
200
201
202
203
204
205
206
  e->writer.write(&e->writer, labcomm_writer_free);
  free(e);
}

void labcomm_encode_type_index(labcomm_encoder_t *e, labcomm_signature_t *s)
{
  int index = get_encoder_index(e, s);
207
  labcomm_encode_packed32(e, index);
Anders Nilsson's avatar
Anders Nilsson committed
208
209
210
}

static int signature_writer(
Riccardo Gaiati's avatar
Riccardo Gaiati committed
211
  labcomm_writer_t *w,
Anders Nilsson's avatar
Anders Nilsson committed
212
213
214
215
216
217
218
219
220
221
  labcomm_writer_action_t action)
{
  switch (action) {
    case labcomm_writer_alloc: {
      w->data_size = 1000;
      w->count = w->data_size;
      w->data = malloc(w->data_size);
      w->pos = 0;
    } break;
    case labcomm_writer_start: {
Riccardo Gaiati's avatar
Riccardo Gaiati committed
222
      w->data_size = 1000;
Anders Nilsson's avatar
Anders Nilsson committed
223
224
225
226
227
      w->count = w->data_size;
      w->data = realloc(w->data, w->data_size);
      w->pos = 0;
    } break;
    case labcomm_writer_continue: {
Riccardo Gaiati's avatar
Riccardo Gaiati committed
228
      w->data_size += 1000;
Anders Nilsson's avatar
Anders Nilsson committed
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
      w->count = w->data_size;
      w->data = realloc(w->data, w->data_size);
    } break;
    case labcomm_writer_end: {
    } break;
    case labcomm_writer_free: {
      free(w->data);
      w->data = 0;
      w->data_size = 0;
      w->count = 0;
      w->pos = 0;
    } break;
    case labcomm_writer_available: {
    } break;
  }
  return 0;

}

static void collect_flat_signature(
  labcomm_decoder_t *decoder,
  labcomm_encoder_t *signature_writer)
{
252
253
254
  //int type = labcomm_decode_int(decoder); 
  int type = labcomm_decode_packed32(decoder); 
//  printf("%s: type=%x\n", __FUNCTION__, type);
Anders Nilsson's avatar
Anders Nilsson committed
255
  if (type >= LABCOMM_USER) {
256
    printf("Implement %s ... (1) for type 0x%x\n", __FUNCTION__, type);
Anders Nilsson's avatar
Anders Nilsson committed
257
  } else {
258
259
    //labcomm_encode_int(signature_writer, type); 
    labcomm_encode_packed32(signature_writer, type); 
Anders Nilsson's avatar
Anders Nilsson committed
260
261
262
    switch (type) {
      case LABCOMM_ARRAY: {
	int dimensions, i;
Riccardo Gaiati's avatar
Riccardo Gaiati committed
263

264
265
	dimensions = labcomm_decode_packed32(decoder); //labcomm_decode_int(decoder); //unpack32
	labcomm_encode_packed32(signature_writer, dimensions); //pack32
Anders Nilsson's avatar
Anders Nilsson committed
266
	for (i = 0 ; i < dimensions ; i++) {
267
268
	  int n = labcomm_decode_packed32(decoder); //labcomm_decode_int(decoder);
	  labcomm_encode_packed32(signature_writer, n); // labcomm_encode_int(signature_writer, n);
Anders Nilsson's avatar
Anders Nilsson committed
269
270
271
272
273
	}
	collect_flat_signature(decoder, signature_writer);
      } break;
      case LABCOMM_STRUCT: {
	int fields, i;
274
275
276
277
	//fields = labcomm_decode_int(decoder); 
	//labcomm_encode_int(signature_writer, fields); 
	fields = labcomm_decode_packed32(decoder); 
	labcomm_encode_packed32(signature_writer, fields); 
Anders Nilsson's avatar
Anders Nilsson committed
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
	for (i = 0 ; i < fields ; i++) {
	  char *name = labcomm_decode_string(decoder);
	  labcomm_encode_string(signature_writer, name);
	  free(name);
	  collect_flat_signature(decoder, signature_writer);
	}
      } 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: {
295
	printf("Implement %s (2) for type 0x%x...\n", __FUNCTION__, type);
Anders Nilsson's avatar
Anders Nilsson committed
296
297
298
299
300
301
      } break;
    }
  }
}

static void do_decoder_register(
Riccardo Gaiati's avatar
Riccardo Gaiati committed
302
303
  labcomm_decoder_t *decoder,
  labcomm_signature_t *signature,
Anders Nilsson's avatar
Anders Nilsson committed
304
305
306
307
  labcomm_decoder_typecast_t type_decoder,
  labcomm_handler_typecast_t handler,
  void *handler_context)
{
Riccardo Gaiati's avatar
Riccardo Gaiati committed
308

Anders Nilsson's avatar
Anders Nilsson committed
309
310
  labcomm_decoder_context_t *context = decoder->context;
  labcomm_sample_entry_t *sample;
Riccardo Gaiati's avatar
Riccardo Gaiati committed
311
  sample = get_sample_by_signature_address(context->sample,
Anders Nilsson's avatar
Anders Nilsson committed
312
313
314
315
316
317
318
					   signature);
  if (!sample) {
    sample = (labcomm_sample_entry_t*)malloc(sizeof(labcomm_sample_entry_t));
    sample->next = context->sample;
    context->sample = sample;
    sample->index = 0;
    sample->signature = signature;
Riccardo Gaiati's avatar
Riccardo Gaiati committed
319
  }
Anders Nilsson's avatar
Anders Nilsson committed
320
321
322
323
324
325
326
327
328
329
330
  sample->decoder = type_decoder;
  sample->handler = handler;
  sample->context = handler_context;
}

static int do_decode_one(labcomm_decoder_t *d)
{
  int result;

  do {
    result = d->reader.read(&d->reader, labcomm_reader_start);
Riccardo Gaiati's avatar
Riccardo Gaiati committed
331
    if (result > 0) {
Anders Nilsson's avatar
Anders Nilsson committed
332
333
      labcomm_decoder_context_t *context = d->context;

334
335
336
//      printf("do_decode_one: result = %x\n", result);
      result = labcomm_decode_packed32(d);
//      printf("do_decode_one: result(2) = %x\n", result);
Anders Nilsson's avatar
Anders Nilsson committed
337
338
339
340
341
      if (result == LABCOMM_TYPEDEF || result == LABCOMM_SAMPLE) {
	labcomm_encoder_t *e = labcomm_encoder_new(signature_writer, 0);
	labcomm_signature_t signature;
	labcomm_sample_entry_t *entry;
	int index;
Riccardo Gaiati's avatar
Riccardo Gaiati committed
342

Anders Nilsson's avatar
Anders Nilsson committed
343
344
	e->writer.write(&e->writer, labcomm_writer_start);
	signature.type = result;
345
	index = labcomm_decode_packed32(d); //int
Anders Nilsson's avatar
Anders Nilsson committed
346
	signature.name = labcomm_decode_string(d);
347
//	printf("do_decode_one: result = %x, index = %x, name=%s\n", result, index, signature.name);
Anders Nilsson's avatar
Anders Nilsson committed
348
349
350
351
352
353
	collect_flat_signature(d, e);
	signature.size = e->writer.pos;
	signature.signature = e->writer.data;
	entry = get_sample_by_signature_value(context->sample, &signature);
	if (! entry) {
	  // Unknown datatype, bail out
Riccardo Gaiati's avatar
Riccardo Gaiati committed
354
	  fprintf(stderr,	"%s: unknown datatype '%s' (id=0x%x)\n",
Anders Nilsson's avatar
Anders Nilsson committed
355
356
		  __FUNCTION__, signature.name, index);
	} else if (entry->index && entry->index != index) {
Riccardo Gaiati's avatar
Riccardo Gaiati committed
357
	  fprintf(stderr,	"%s: index mismatch '%s' (id=0x%x != 0x%x)\n",
Anders Nilsson's avatar
Anders Nilsson committed
358
359
360
361
362
363
364
365
366
367
368
369
370
		  __FUNCTION__, signature.name, entry->index, index);
	} else {
	  entry->index = index;
	}
	free(signature.name);
	e->writer.write(&e->writer, labcomm_writer_end);
	if (!entry) {
	  // No handler for found type, bail out (after cleanup)
	  result = -ENOENT;
	}
	labcomm_encoder_free(e);
      } else {
	labcomm_sample_entry_t *entry;
Riccardo Gaiati's avatar
Riccardo Gaiati committed
371

Anders Nilsson's avatar
Anders Nilsson committed
372
373
	entry = get_sample_by_index(context->sample, result);
	if (!entry) {
Riccardo Gaiati's avatar
Riccardo Gaiati committed
374
	  fprintf(stderr,	"%s: type not found (id=0x%x)\n",
Anders Nilsson's avatar
Anders Nilsson committed
375
376
377
378
379
380
381
382
		  __FUNCTION__, result);
	  result = -ENOENT;
	} else {
	  entry->decoder(d, entry->handler, entry->context);
	}
      }
    }
    d->reader.read(&d->reader, labcomm_reader_end);
Riccardo Gaiati's avatar
Riccardo Gaiati committed
383
  } while (result > 0 && result < LABCOMM_USER);
Anders Nilsson's avatar
Anders Nilsson committed
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
  return result;
}

labcomm_decoder_t *labcomm_decoder_new(
  int (*reader)(labcomm_reader_t *, labcomm_reader_action_t),
  void *reader_context)
{
  labcomm_decoder_t *result = malloc(sizeof(labcomm_decoder_t));
  if (result) {
    labcomm_decoder_context_t *context =
      (labcomm_decoder_context_t*)malloc(sizeof(labcomm_decoder_context_t));
    context->sample = 0;
    result->context = context;
    result->reader.context = reader_context;
    result->reader.data = 0;
    result->reader.data_size = 0;
    result->reader.count = 0;
    result->reader.pos = 0;
    result->reader.read = reader;
    result->reader.read(&result->reader, labcomm_reader_alloc);
    result->do_register = do_decoder_register;
    result->do_decode_one = do_decode_one;
  }
  return result;
}

void labcomm_internal_decoder_register(
Riccardo Gaiati's avatar
Riccardo Gaiati committed
411
412
  labcomm_decoder_t *d,
  labcomm_signature_t *signature,
Anders Nilsson's avatar
Anders Nilsson committed
413
414
415
416
417
418
419
420
421
422
423
424
425
426
  labcomm_decoder_typecast_t type_decoder,
  labcomm_handler_typecast_t handler,
  void *handler_context)
{
  if (d && d->do_register) {
    d->do_register(d, signature, type_decoder, handler, handler_context);
  } else {
    printf("Decoder is missing do_register\n");
  }
}

int labcomm_decoder_decode_one(labcomm_decoder_t *d)
{
  int result = -1;
Riccardo Gaiati's avatar
Riccardo Gaiati committed
427
428
  if (d && d->do_decode_one)
  {
Anders Nilsson's avatar
Anders Nilsson committed
429
    result = d->do_decode_one(d);
Riccardo Gaiati's avatar
Riccardo Gaiati committed
430
431
432
  }
  else
  {
Anders Nilsson's avatar
Anders Nilsson committed
433
434
435
436
437
438
439
440
441
442
443
    printf("Decoder is missing do_decode_one\n");
  }
  return result;
}

void labcomm_decoder_run(labcomm_decoder_t *d)
{
  while (labcomm_decoder_decode_one(d) > 0) {
  }
}

Riccardo Gaiati's avatar
Riccardo Gaiati committed
444
void labcomm_decoder_free(labcomm_decoder_t* d)
Anders Nilsson's avatar
Anders Nilsson committed
445
446
447
448
{
  d->reader.read(&d->reader, labcomm_reader_free);
  free(d);
}