labcomm.c 20.2 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
/*
  labcomm.c -- runtime for handling encoding and decoding of
               labcomm samples.

  Copyright 2006-2013 Anders Blomdell <anders.blomdell@control.lth.se>

  This program 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.

  This program 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/>.
*/

21
22
23
#ifdef LABCOMM_COMPAT
  #include LABCOMM_COMPAT
#else
Oscar Olsson's avatar
Oscar Olsson committed
24
  #include <stdio.h>
25
  #include <strings.h>
Anders Nilsson's avatar
Anders Nilsson committed
26
#endif
Oscar Olsson's avatar
Oscar Olsson committed
27

28
29
#include <errno.h>
#include <string.h>
30
31
#include <stdarg.h>

Anders Nilsson's avatar
Anders Nilsson committed
32
33
#include "labcomm.h"
#include "labcomm_private.h"
34
#include "labcomm_ioctl.h"
35
#include "labcomm_dynamic_buffer_writer.h"
Anders Nilsson's avatar
Anders Nilsson committed
36

37
38
#define LABCOMM_VERSION "LabComm2013"

Anders Nilsson's avatar
Anders Nilsson committed
39
40
41
42
43
44
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;
45
  labcomm_encoder_function encode;
Anders Nilsson's avatar
Anders Nilsson committed
46
47
48
  void *context;
} labcomm_sample_entry_t;

49
50
51
52
53
#ifndef LABCOMM_ENCODER_LINEAR_SEARCH
extern  labcomm_signature_t labcomm_first_signature;
extern  labcomm_signature_t labcomm_last_signature;
#endif

Anders Nilsson's avatar
Anders Nilsson committed
54
typedef struct labcomm_encoder_context {
55
#ifdef LABCOMM_ENCODER_LINEAR_SEARCH
Anders Nilsson's avatar
Anders Nilsson committed
56
57
  labcomm_sample_entry_t *sample;
  int index;
58
59
60
#else
  labcomm_sample_entry_t *by_section;
#endif
Anders Nilsson's avatar
Anders Nilsson committed
61
62
63
64
65
66
} labcomm_encoder_context_t;

typedef struct labcomm_decoder_context {
  labcomm_sample_entry_t *sample;
} labcomm_decoder_context_t;

Oscar Olsson's avatar
Oscar Olsson committed
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
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
void labcomm_register_error_handler_encoder(struct labcomm_encoder *encoder, labcomm_error_handler_callback callback)
{
 encoder->on_error = callback; 
 encoder->writer.on_error = callback; 
}

void labcomm_register_error_handler_decoder(struct labcomm_decoder *decoder, labcomm_error_handler_callback callback)
{
 decoder->on_error = callback; 
 decoder->reader.on_error = callback; 
}

/* Error strings. _must_ be the same order as in enum labcomm_error */
const char *labcomm_error_strings[] = { 
  "Enum begin guard. DO NO use this as an error.",
  "Encoder has no registration for this signature.",
  "Encoder is missing do_register",
  "Encoder is missing do_encode",
  "The labcomm buffer is full and it.",
  "Decoder is missing do_register",
  "Decoder is missing do_decode_one",
  "Decoder: Unknown datatype",
  "Decoder: index mismatch",
  "Decoder: type not found",
  "This function is not yet implemented.",
  "User defined error.",
  "Could not allocate memory.",
  "Enum end guard. DO NO use this as an error."
};

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_ENUM_BEGIN_GUARD && error_id <= LABCOMM_ERROR_ENUM_END_GUARD) {
    error_str = labcomm_error_strings[error_id];
  }
  return error_str;
}

void labcomm_decoder_register_new_datatype_handler(struct labcomm_decoder *d, labcomm_handle_new_datatype_callback on_new_datatype)
{
	d->on_new_datatype = on_new_datatype;
}

int on_new_datatype(labcomm_decoder_t *d, labcomm_signature_t *sig)
{
	  d->on_error(LABCOMM_ERROR_DEC_UNKNOWN_DATATYPE, 4, "%s(): unknown datatype '%s'\n", __FUNCTION__, sig->name);
	  return 0;
}

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
}


Anders Nilsson's avatar
Anders Nilsson committed
144
145
146
147
148
149
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
150

Anders Nilsson's avatar
Anders Nilsson committed
151
152
153
154
155
156
157
158
159
160
161
162
163
  }
  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
164
	bcmp((void*)p->signature->signature, (void*)signature->signature,
Anders Nilsson's avatar
Anders Nilsson committed
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
	     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;
}

182
183
#ifdef LABCOMM_ENCODER_LINEAR_SEARCH

184
static int get_encoder_index_by_search(
Anders Nilsson's avatar
Anders Nilsson committed
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
  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;
}

201
202
#else

203
static int get_encoder_index_by_section(
204
205
206
  labcomm_encoder_t *e,
  labcomm_signature_t *s)
{
207
  int result = -ENOENT;
208
209
210
211
212
213
214
215
  if (&labcomm_first_signature <= s && s <= &labcomm_last_signature) {
    //fprintf(stderr, "%d\n", (int)(s - &labcomm_start));
    result = s - &labcomm_first_signature + LABCOMM_USER;
  }
  return result;
}

#endif
216
217
218
219
220
221
222
223
224
225
static int get_encoder_index(
  labcomm_encoder_t *e,
  labcomm_signature_t *s)
{
#ifdef LABCOMM_ENCODER_LINEAR_SEARCH
  return get_encoder_index_by_search(e, s);
#else
  return get_encoder_index_by_section(e, s);
#endif
}
226

227
228
229
230
void labcomm_encode_signature(struct labcomm_encoder *e,
                              labcomm_signature_t *signature) 
{
  int i;
231
  e->writer.write(&e->writer, labcomm_writer_start_signature);
232
  labcomm_encode_packed32(e, signature->type);
233
234
  labcomm_encode_packed32(e, get_encoder_index(e, signature));

235
236
237
238
239
240
241
242
  labcomm_encode_string(e, signature->name);
  for (i = 0 ; i < signature->size ; i++) {
    if (e->writer.pos >= e->writer.count) {
      e->writer.write(&e->writer, labcomm_writer_continue);
    }
    e->writer.data[e->writer.pos] = signature->signature[i];
    e->writer.pos++;
  }
243
  e->writer.write(&e->writer, labcomm_writer_end_signature);
244
245
}

246
247
248
#ifdef LABCOMM_ENCODER_LINEAR_SEARCH
static int encoder_add_signature_by_search(struct labcomm_encoder *e,
					   labcomm_signature_t *signature,
249
					   labcomm_encoder_function encode)
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
{
  int result;
  labcomm_encoder_context_t *context = e->context;
  labcomm_sample_entry_t *sample;

  sample = (labcomm_sample_entry_t*)malloc(sizeof(labcomm_sample_entry_t));
  if (sample == NULL) {
    result = -ENOMEM;
  } else {
    sample->next = context->sample;
    sample->index = context->index;
    sample->signature = signature;
    sample->encode = encode;
    context->index++;
    context->sample = sample;
    result = sample->index;
  }
  return result;
}
#endif

#ifndef LABCOMM_ENCODER_LINEAR_SEARCH
static int encoder_add_signature_by_section(struct labcomm_encoder *e,
					    labcomm_signature_t *s,
274
					    labcomm_encoder_function encode)
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
{
  int result = -ENOENT;
  
  if (&labcomm_first_signature <= s && s <= &labcomm_last_signature) {
    /* Signature is in right linker section */
    labcomm_encoder_context_t *context = e->context;
    int index = s - &labcomm_first_signature;

    if (context->by_section == NULL) {
      int n = &labcomm_last_signature - &labcomm_first_signature;
      context->by_section = malloc(n * sizeof(context->by_section[0]));
    }
    if (context->by_section == NULL) {
      result = -ENOMEM;
      goto out;
    }
    context->by_section[index].next = NULL;
    context->by_section[index].index = index + LABCOMM_USER;
    context->by_section[index].signature = s;
    context->by_section[index].encode = encode;
    result = context->by_section[index].index;
  }
out:
  return result;
}
#endif

static int encoder_add_signature(struct labcomm_encoder *e,
				  labcomm_signature_t *signature,
304
				  labcomm_encoder_function encode)
305
306
307
308
309
310
311
312
313
314
315
{
  int index = -ENOENT;

#ifdef LABCOMM_ENCODER_LINEAR_SEARCH
  index = encoder_add_signature_by_search(e, signature, encode);
#else
  index = encoder_add_signature_by_section(e, signature, encode);
#endif
  return index;
}

Riccardo Gaiati's avatar
Riccardo Gaiati committed
316
static void do_encoder_register(struct labcomm_encoder *e,
Anders Nilsson's avatar
Anders Nilsson committed
317
				labcomm_signature_t *signature,
318
				labcomm_encoder_function encode)
Anders Nilsson's avatar
Anders Nilsson committed
319
320
321
{
  if (signature->type == LABCOMM_SAMPLE) {
    if (get_encoder_index(e, signature) == 0) {
322
323
324
325
326
327
328
329
330
331
332
333
334
      int index = encoder_add_signature(e, signature, encode);
      
      if (index > 0) {
	struct labcomm_ioctl_register_signature ioctl_data;
	int err;
	
	ioctl_data.index = index;
	ioctl_data.signature = signature;	
	err = labcomm_encoder_ioctl(e, LABCOMM_IOCTL_REGISTER_SIGNATURE,
				    &ioctl_data);
	if (err != 0) {
	  labcomm_encode_signature(e, signature);
	}
335
      }
Anders Nilsson's avatar
Anders Nilsson committed
336
337
338
339
    }
  }
}

340
/*
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
static labcomm_sample_entry_t *encoder_get_sample_by_signature_address(
  labcomm_encoder_t *encoder,
  labcomm_signature_t *s)
{
  labcomm_sample_entry_t *result = NULL;
  labcomm_encoder_context_t *context = encoder->context;
  
#ifndef LABCOMM_ENCODER_LINEAR_SEARCH
  if (&labcomm_first_signature <= s && s <= &labcomm_last_signature) {
    result = &context->by_section[s - &labcomm_first_signature];
  }
#else
  result = get_sample_by_signature_address(context->sample, s);
#endif
  return result;
}
357
*/
358
						    
359
360
static int do_encode(
  labcomm_encoder_t *e,
Riccardo Gaiati's avatar
Riccardo Gaiati committed
361
  labcomm_signature_t *signature,
362
  labcomm_encoder_function encode,
Anders Nilsson's avatar
Anders Nilsson committed
363
364
  void *value)
{
365
366
  int result;
  labcomm_writer_start_t lws;
367
  
368
369
370
371
372
373
374
375
376
377
378
379
380
  lws.encoder = e;
  lws.index = get_encoder_index(e, signature);
  lws.signature = signature;
  lws.value = value;
  result = e->writer.write(&e->writer, labcomm_writer_start, &lws);
  if (result == -EALREADY) { result = 0; goto out; }
  if (result != 0) { goto out; }
  result = labcomm_encode_packed32(e, lws.index);
  if (result != 0) { goto out; }
  result = encode(e, value);
out:
  e->writer.write(&e->writer, labcomm_writer_end, &lws);
  return result;
Anders Nilsson's avatar
Anders Nilsson committed
381
382
383
}

labcomm_encoder_t *labcomm_encoder_new(
Sven Robertz's avatar
Sven Robertz committed
384
  int (*writer)(labcomm_writer_t *, labcomm_writer_action_t, ...),
Anders Nilsson's avatar
Anders Nilsson committed
385
386
387
388
389
390
391
  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));
392
#ifdef LABCOMM_ENCODER_LINEAR_SEARCH
Oscar Olsson's avatar
Oscar Olsson committed
393
    context->sample = NULL;
Anders Nilsson's avatar
Anders Nilsson committed
394
    context->index = LABCOMM_USER;
395
396
397
#else
    context->by_section = NULL;
#endif
Anders Nilsson's avatar
Anders Nilsson committed
398
399
400
401
402
403
    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;
404
    result->writer.error = 0;
Anders Nilsson's avatar
Anders Nilsson committed
405
    result->writer.write = writer;
406
    result->writer.ioctl = NULL;
Oscar Olsson's avatar
Oscar Olsson committed
407
    result->writer.on_error = on_error_fprintf;
Anders Nilsson's avatar
Anders Nilsson committed
408
409
    result->do_register = do_encoder_register;
    result->do_encode = do_encode;
Oscar Olsson's avatar
Oscar Olsson committed
410
    result->on_error = on_error_fprintf;
411
    result->writer.write(&result->writer, labcomm_writer_alloc);
Anders Nilsson's avatar
Anders Nilsson committed
412
413
414
415
416
  }
  return result;
}

void labcomm_internal_encoder_register(
Riccardo Gaiati's avatar
Riccardo Gaiati committed
417
  labcomm_encoder_t *e,
Anders Nilsson's avatar
Anders Nilsson committed
418
  labcomm_signature_t *signature,
419
  labcomm_encoder_function encode)
Anders Nilsson's avatar
Anders Nilsson committed
420
{
Oscar Olsson's avatar
Oscar Olsson committed
421
422
  // Will segfault if e == NULL.
  if (e->do_register) {
Anders Nilsson's avatar
Anders Nilsson committed
423
424
    e->do_register(e, signature, encode);
  } else {
Oscar Olsson's avatar
Oscar Olsson committed
425
    e->on_error(LABCOMM_ERROR_ENC_MISSING_DO_REG, 0);
Anders Nilsson's avatar
Anders Nilsson committed
426
427
428
  }
}

429
int labcomm_internal_encode(
Riccardo Gaiati's avatar
Riccardo Gaiati committed
430
431
  labcomm_encoder_t *e,
  labcomm_signature_t *signature,
432
  labcomm_encoder_function encode,
Anders Nilsson's avatar
Anders Nilsson committed
433
434
  void *value)
{
Oscar Olsson's avatar
Oscar Olsson committed
435
  if (e->do_encode) {
436
    return e->do_encode(e, signature, encode, value);
Anders Nilsson's avatar
Anders Nilsson committed
437
  } else {
Oscar Olsson's avatar
Oscar Olsson committed
438
    e->on_error(LABCOMM_ERROR_ENC_MISSING_DO_ENCODE, 0);
Riccardo Gaiati's avatar
Riccardo Gaiati committed
439
  }
440
  return 0;
Anders Nilsson's avatar
Anders Nilsson committed
441
}
Riccardo Gaiati's avatar
Riccardo Gaiati committed
442
443

void labcomm_encoder_free(labcomm_encoder_t* e)
Anders Nilsson's avatar
Anders Nilsson committed
444
445
{
  e->writer.write(&e->writer, labcomm_writer_free);
446
  labcomm_encoder_context_t *context = (labcomm_encoder_context_t *) e->context;
Oscar Olsson's avatar
Oscar Olsson committed
447

448
449
450
451
452
453
454
455
456
457
458
#ifdef LABCOMM_ENCODER_LINEAR_SEARCH
  labcomm_sample_entry_t *entry = context->sample;
  labcomm_sample_entry_t *entry_next;
  while (entry != NULL) {
    entry_next = entry->next;
    free(entry);
    entry = entry_next;
  }
#else
  free(context->by_section);
#endif
Oscar Olsson's avatar
Oscar Olsson committed
459
  free(e->context);
Anders Nilsson's avatar
Anders Nilsson committed
460
461
462
  free(e);
}

463
464
465
int labcomm_encoder_ioctl(struct labcomm_encoder *encoder, 
                           int action,
                           ...)
Anders Nilsson's avatar
Anders Nilsson committed
466
{
467
468
469
470
471
472
473
474
475
476
  int result = -ENOTSUP;
  
  if (encoder->writer.ioctl != NULL) {
    va_list va;
    
    va_start(va, action);
    result = encoder->writer.ioctl(&encoder->writer, action, va);
    va_end(va);
  }
  return result;
Anders Nilsson's avatar
Anders Nilsson committed
477
478
479
480
481
482
}

static void collect_flat_signature(
  labcomm_decoder_t *decoder,
  labcomm_encoder_t *signature_writer)
{
483
484
485
  //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
486
  if (type >= LABCOMM_USER) {
487
488
    decoder->on_error(LABCOMM_ERROR_UNIMPLEMENTED_FUNC, 3,
			"Implement %s ... (1) for type 0x%x\n", __FUNCTION__, type);
Anders Nilsson's avatar
Anders Nilsson committed
489
  } else {
490
491
    //labcomm_encode_int(signature_writer, type); 
    labcomm_encode_packed32(signature_writer, type); 
Anders Nilsson's avatar
Anders Nilsson committed
492
493
494
    switch (type) {
      case LABCOMM_ARRAY: {
	int dimensions, i;
Riccardo Gaiati's avatar
Riccardo Gaiati committed
495

496
497
	dimensions = labcomm_decode_packed32(decoder); //labcomm_decode_int(decoder); //unpack32
	labcomm_encode_packed32(signature_writer, dimensions); //pack32
Anders Nilsson's avatar
Anders Nilsson committed
498
	for (i = 0 ; i < dimensions ; i++) {
499
500
	  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
501
502
503
504
505
	}
	collect_flat_signature(decoder, signature_writer);
      } break;
      case LABCOMM_STRUCT: {
	int fields, i;
506
507
508
509
	//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
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
	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: {
527
528
        decoder->on_error(LABCOMM_ERROR_UNIMPLEMENTED_FUNC, 3,
				"Implement %s (2) for type 0x%x...\n", __FUNCTION__, type);
Anders Nilsson's avatar
Anders Nilsson committed
529
530
531
532
533
534
      } break;
    }
  }
}

static void do_decoder_register(
Riccardo Gaiati's avatar
Riccardo Gaiati committed
535
536
  labcomm_decoder_t *decoder,
  labcomm_signature_t *signature,
Anders Nilsson's avatar
Anders Nilsson committed
537
538
539
540
  labcomm_decoder_typecast_t type_decoder,
  labcomm_handler_typecast_t handler,
  void *handler_context)
{
Riccardo Gaiati's avatar
Riccardo Gaiati committed
541

Anders Nilsson's avatar
Anders Nilsson committed
542
543
  labcomm_decoder_context_t *context = decoder->context;
  labcomm_sample_entry_t *sample;
Riccardo Gaiati's avatar
Riccardo Gaiati committed
544
  sample = get_sample_by_signature_address(context->sample,
Anders Nilsson's avatar
Anders Nilsson committed
545
546
547
548
549
550
551
					   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
552
  }
Anders Nilsson's avatar
Anders Nilsson committed
553
554
555
556
557
558
559
560
561
562
563
  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
564
    if (result > 0) {
Anders Nilsson's avatar
Anders Nilsson committed
565
566
      labcomm_decoder_context_t *context = d->context;

567
568
569
//      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
570
      if (result == LABCOMM_TYPEDEF || result == LABCOMM_SAMPLE) {
571
572
	/* TODO: should the labcomm_dynamic_buffer_writer be 
	   a permanent part of labcomm_decoder? */
573
574
	labcomm_encoder_t *e = labcomm_encoder_new(
	  labcomm_dynamic_buffer_writer, 0);
Anders Nilsson's avatar
Anders Nilsson committed
575
	labcomm_signature_t signature;
576
577
	labcomm_sample_entry_t *entry = NULL;
	int index, err;
Riccardo Gaiati's avatar
Riccardo Gaiati committed
578

579
	index = labcomm_decode_packed32(d); //int
Anders Nilsson's avatar
Anders Nilsson committed
580
	signature.name = labcomm_decode_string(d);
581
582
583
584
	signature.type = result;
	e->writer.write(&e->writer, labcomm_writer_start);
	/* printf("do_decode_one: result = %x, index = %x, name=%s\n", 
	   result, index, signature.name); */
Anders Nilsson's avatar
Anders Nilsson committed
585
	collect_flat_signature(d, e);
586
587
588
589
590
591
592
593
594
595
596
597
598
	e->writer.write(&e->writer, labcomm_writer_end);
	err = labcomm_encoder_ioctl(e, LABCOMM_IOCTL_WRITER_GET_BYTES_WRITTEN,
				    &signature.size);
	if (err < 0) {
	  printf("Failed to get size: %s\n", strerror(-err));
	  goto free_signature_name;
	}
	err = labcomm_encoder_ioctl(e, LABCOMM_IOCTL_WRITER_GET_BYTE_POINTER,
				    &signature.signature);
	if (err < 0) {
	  printf("Failed to get pointer: %s\n", strerror(-err));
	  goto free_signature_name;
	}
Anders Nilsson's avatar
Anders Nilsson committed
599
600
	entry = get_sample_by_signature_value(context->sample, &signature);
	if (! entry) {
601
602
	  /* Unknown datatype, bail out */
	  d->on_new_datatype(d, &signature);
Anders Nilsson's avatar
Anders Nilsson committed
603
	} else if (entry->index && entry->index != index) {
604
605
606
          d->on_error(LABCOMM_ERROR_DEC_INDEX_MISMATCH, 5, 
		      "%s(): index mismatch '%s' (id=0x%x != 0x%x)\n", 
		      __FUNCTION__, signature.name, entry->index, index);
Anders Nilsson's avatar
Anders Nilsson committed
607
	} else {
608
	  // TODO unnessesary, since entry->index == index in above if statement
Anders Nilsson's avatar
Anders Nilsson committed
609
610
	  entry->index = index;
	}
611
      free_signature_name:
Anders Nilsson's avatar
Anders Nilsson committed
612
	free(signature.name);
613
	labcomm_encoder_free(e);
Anders Nilsson's avatar
Anders Nilsson committed
614
615
616
617
618
619
	if (!entry) {
	  // No handler for found type, bail out (after cleanup)
	  result = -ENOENT;
	}
      } else {
	labcomm_sample_entry_t *entry;
Riccardo Gaiati's avatar
Riccardo Gaiati committed
620

Anders Nilsson's avatar
Anders Nilsson committed
621
622
	entry = get_sample_by_index(context->sample, result);
	if (!entry) {
Oscar Olsson's avatar
Oscar Olsson committed
623
624
625
	  // printf("Error: %s: type not found (id=0x%x)\n",
		  //__FUNCTION__, result);
          d->on_error(LABCOMM_ERROR_DEC_TYPE_NOT_FOUND, 3, "%s(): type not found (id=0x%x)\n", __FUNCTION__, result);
Anders Nilsson's avatar
Anders Nilsson committed
626
627
628
629
630
631
632
	  result = -ENOENT;
	} else {
	  entry->decoder(d, entry->handler, entry->context);
	}
      }
    }
    d->reader.read(&d->reader, labcomm_reader_end);
633
634
    /* TODO: should we really loop, or is it OK to
       return after a typedef/sample */
Riccardo Gaiati's avatar
Riccardo Gaiati committed
635
  } while (result > 0 && result < LABCOMM_USER);
Anders Nilsson's avatar
Anders Nilsson committed
636
637
638
639
  return result;
}

labcomm_decoder_t *labcomm_decoder_new(
640
  int (*reader)(labcomm_reader_t *, labcomm_reader_action_t, ...),
Anders Nilsson's avatar
Anders Nilsson committed
641
642
643
644
645
646
647
648
649
650
651
652
653
654
  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;
655
    result->reader.ioctl = NULL;
Oscar Olsson's avatar
Oscar Olsson committed
656
    result->reader.on_error = on_error_fprintf;
Anders Nilsson's avatar
Anders Nilsson committed
657
658
    result->do_register = do_decoder_register;
    result->do_decode_one = do_decode_one;
Oscar Olsson's avatar
Oscar Olsson committed
659
    result->on_error = on_error_fprintf;
660
    result->on_new_datatype = on_new_datatype;
661
    result->reader.read(&result->reader, labcomm_reader_alloc, LABCOMM_VERSION);
Anders Nilsson's avatar
Anders Nilsson committed
662
663
664
665
666
  }
  return result;
}

void labcomm_internal_decoder_register(
Riccardo Gaiati's avatar
Riccardo Gaiati committed
667
668
  labcomm_decoder_t *d,
  labcomm_signature_t *signature,
Anders Nilsson's avatar
Anders Nilsson committed
669
670
671
672
  labcomm_decoder_typecast_t type_decoder,
  labcomm_handler_typecast_t handler,
  void *handler_context)
{
Oscar Olsson's avatar
Oscar Olsson committed
673
674
  // Will segfault if d == NULL
  if (d->do_register) {
Anders Nilsson's avatar
Anders Nilsson committed
675
676
    d->do_register(d, signature, type_decoder, handler, handler_context);
  } else {
Oscar Olsson's avatar
Oscar Olsson committed
677
    d->on_error(LABCOMM_ERROR_DEC_MISSING_DO_REG, 0);
Anders Nilsson's avatar
Anders Nilsson committed
678
679
680
681
682
683
  }
}

int labcomm_decoder_decode_one(labcomm_decoder_t *d)
{
  int result = -1;
Oscar Olsson's avatar
Oscar Olsson committed
684
685
  // Will segfault if decoder == NULL.
  if (d->do_decode_one)
Riccardo Gaiati's avatar
Riccardo Gaiati committed
686
  {
Anders Nilsson's avatar
Anders Nilsson committed
687
    result = d->do_decode_one(d);
Riccardo Gaiati's avatar
Riccardo Gaiati committed
688
689
690
  }
  else
  {
Oscar Olsson's avatar
Oscar Olsson committed
691
    d->on_error(LABCOMM_ERROR_DEC_MISSING_DO_DECODE_ONE, 0);
Anders Nilsson's avatar
Anders Nilsson committed
692
693
694
695
696
697
698
699
700
701
  }
  return result;
}

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

Riccardo Gaiati's avatar
Riccardo Gaiati committed
702
void labcomm_decoder_free(labcomm_decoder_t* d)
Anders Nilsson's avatar
Anders Nilsson committed
703
704
{
  d->reader.read(&d->reader, labcomm_reader_free);
Oscar Olsson's avatar
Oscar Olsson committed
705
706
707
708
709
710
711
712
713
714
715
  labcomm_decoder_context_t *context = (labcomm_decoder_context_t *) d->context;
  labcomm_sample_entry_t *entry = context->sample;
  labcomm_sample_entry_t *entry_next;

  while (entry != NULL) {
    entry_next = entry->next;
    free(entry);
    entry = entry_next;
  }

  free(d->context);
Anders Nilsson's avatar
Anders Nilsson committed
716
717
  free(d);
}