labcomm.c 25.9 KB
Newer Older
1
2
3
4
5
6
/*
  labcomm.c -- runtime for handling encoding and decoding of
               labcomm samples.

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

7
8
9
  This file is part of LabComm.

  LabComm is free software: you can redistribute it and/or modify
10
11
12
13
  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.

14
  LabComm is distributed in the hope that it will be useful,
15
16
17
18
19
20
21
22
  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/>.
*/

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

30
31
#include <errno.h>
#include <string.h>
32
#include <stdarg.h>
33
#include <stddef.h>
34

Anders Nilsson's avatar
Anders Nilsson committed
35
36
#include "labcomm.h"
#include "labcomm_private.h"
37
#include "labcomm_ioctl.h"
38
#include "labcomm_dynamic_buffer_writer.h"
Anders Nilsson's avatar
Anders Nilsson committed
39

40
#define LABCOMM_VERSION "LabComm2013"
41

42
43
struct labcomm_decoder {
  void *context;
44
  struct labcomm_reader *reader;
45
  struct labcomm_lock *lock;
46
47
  labcomm_error_handler_callback on_error;
  labcomm_handle_new_datatype_callback on_new_datatype;
48
49
  LABCOMM_SIGNATURE_ARRAY_DEF(local_to_remote, int);
  LABCOMM_SIGNATURE_ARRAY_DEF(remote_to_local, int);
50
51
52
53
};

struct labcomm_encoder {
  void *context;
54
  struct labcomm_writer *writer;
55
  struct labcomm_lock *lock;
56
57
58
59
60
61
62
63
  struct labcomm_encoder *is_deferred;
  int busy;
  int waiting;
  struct encoder_alloc_action {
    struct encoder_alloc_action *next;
    void (*action)(struct labcomm_encoder *encoder, void *context);
    void *context;
  } *alloc_action;
64
  labcomm_error_handler_callback on_error;
65
  LABCOMM_SIGNATURE_ARRAY_DEF(registered, int);
66
67
};

68
struct labcomm_sample_entry {
Anders Nilsson's avatar
Anders Nilsson committed
69
70
  struct labcomm_sample_entry *next;
  int index;
71
72
73
  struct labcomm_signature *signature;
  labcomm_decoder_function decoder;
  labcomm_handler_function handler;
74
  labcomm_encoder_function encode;
Anders Nilsson's avatar
Anders Nilsson committed
75
  void *context;
76
};
Anders Nilsson's avatar
Anders Nilsson committed
77

78
79
extern  struct labcomm_signature labcomm_first_signature;
extern  struct labcomm_signature labcomm_last_signature;
80

81
82
83
struct labcomm_encoder_context {
  struct labcomm_sample_entry *by_section;
};
Anders Nilsson's avatar
Anders Nilsson committed
84

85
86
87
struct labcomm_decoder_context {
  struct labcomm_sample_entry *sample;
};
Anders Nilsson's avatar
Anders Nilsson committed
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
/* Lock wrappers */
#define CONDCALL_lock(lock, ...) lock
#define CONDCALL(func, ...)					\
  if (CONDCALL_lock(__VA_ARGS__) &&				\
      CONDCALL_lock(__VA_ARGS__)->action->func) {		\
  return CONDCALL_lock(__VA_ARGS__)->action->func(__VA_ARGS__);	\
  }								\
  return -ENOSYS;

int labcomm_lock_free(struct labcomm_lock *lock) {
  CONDCALL(free, lock);
}

int labcomm_lock_acquire(struct labcomm_lock *lock)
{
  CONDCALL(acquire, lock);
}

int labcomm_lock_release(struct labcomm_lock *lock)
{
  CONDCALL(release, lock);
}

int labcomm_lock_wait(struct labcomm_lock *lock, useconds_t usec){
  CONDCALL(wait, lock, usec);
}

int labcomm_lock_notify(struct labcomm_lock *lock)
{
  CONDCALL(notify, lock);
}

#undef CONDCALL
#undef CONDCALL_lock

124
/* Unwrapping reader/writer functions */
125
#define UNWRAP_ac(rw, ac, ...) ac
126
127
#define UNWRAP(func, ...)	     \
  while (1) {								\
128
129
130
131
    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;		\
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
  }

int labcomm_reader_alloc(struct labcomm_reader *r, 
                         struct labcomm_reader_action_context *action_context, 
                         struct labcomm_decoder *decoder, 
                         char *labcomm_version)
{
  UNWRAP(alloc, r, action_context, decoder, 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, 
149
150
151
                         struct labcomm_reader_action_context *action_context,
			 int index, struct labcomm_signature *signature,
			 void *value)
152
{
153
  UNWRAP(start, r, action_context, index, signature, value);
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
}

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 index, 
                         struct labcomm_signature *signature, 
                         uint32_t ioctl_action, va_list args)
{
  UNWRAP(ioctl, r, action_context, index, signature, ioctl_action, args);
}

int labcomm_writer_alloc(struct labcomm_writer *w, 
                         struct labcomm_writer_action_context *action_context, 
                         struct labcomm_encoder *encoder, 
180
181
                         char *labcomm_version,
			 labcomm_encoder_enqueue enqueue)
182
{
183
  UNWRAP(alloc, w, action_context, encoder, labcomm_version, enqueue);
184
185
186
187
188
189
190
191
192
193
194
195
196
}

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)
{
197
  UNWRAP(start, w, action_context, index, signature, value);
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
}

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);
} 

221
222
#undef UNWRAP
#undef UNWRAP_ac
223
224
225



Oscar Olsson's avatar
Oscar Olsson committed
226
227
228
229
230
231
232
233
234
235
void labcomm_register_error_handler_encoder(struct labcomm_encoder *encoder, labcomm_error_handler_callback callback)
{
 encoder->on_error = callback; 
}

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

Anders Blomdell's avatar
Anders Blomdell committed
236
237
238
239
static const char *labcomm_error_string[] = { 
#define LABCOMM_ERROR(name, description) description ,
#include "labcomm_error.h"
#undef LABCOMM_ERROR
Oscar Olsson's avatar
Oscar Olsson committed
240
};
Anders Blomdell's avatar
Anders Blomdell committed
241
242
243
static const int labcomm_error_string_count = (sizeof(labcomm_error_string) / 
					       sizeof(labcomm_error_string[0]));

Oscar Olsson's avatar
Oscar Olsson committed
244
245
246
247
248

const char *labcomm_error_get_str(enum labcomm_error error_id)
{
  const char *error_str = NULL;
  // Check if this is a known error ID.
Anders Blomdell's avatar
Anders Blomdell committed
249
250
  if (0 <= error_id && error_id < labcomm_error_string_count) {
    error_str = labcomm_error_string[error_id];
Oscar Olsson's avatar
Oscar Olsson committed
251
252
253
254
255
256
257
258
259
  }
  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;
}

260
int on_new_datatype(struct labcomm_decoder *d, struct labcomm_signature *sig)
Oscar Olsson's avatar
Oscar Olsson committed
261
{
262
  d->on_error(LABCOMM_ERROR_DEC_UNKNOWN_DATATYPE, 4, "%s(): unknown datatype '%s'\n", __FUNCTION__, sig->name);
Oscar Olsson's avatar
Oscar Olsson committed
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
	  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
}


292
293
294
static struct labcomm_sample_entry *get_sample_by_signature_address(
  struct labcomm_sample_entry *head,
  struct labcomm_signature *signature)
Anders Nilsson's avatar
Anders Nilsson committed
295
{
296
  struct labcomm_sample_entry *p;
Anders Nilsson's avatar
Anders Nilsson committed
297
  for (p = head ; p && p->signature != signature ; p = p->next) {
Riccardo Gaiati's avatar
Riccardo Gaiati committed
298

Anders Nilsson's avatar
Anders Nilsson committed
299
300
301
302
  }
  return p;
}

303
304
305
static struct labcomm_sample_entry *get_sample_by_signature_value(
  struct labcomm_sample_entry *head,
  struct labcomm_signature *signature)
Anders Nilsson's avatar
Anders Nilsson committed
306
{
307
  struct labcomm_sample_entry *p;
Anders Nilsson's avatar
Anders Nilsson committed
308
309
310
311
  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
312
	bcmp((void*)p->signature->signature, (void*)signature->signature,
Anders Nilsson's avatar
Anders Nilsson committed
313
314
315
316
317
318
319
	     signature->size) == 0) {
      break;
    }
  }
  return p;
}

320
321
static struct labcomm_sample_entry *get_sample_by_index(
  struct labcomm_sample_entry *head,
Anders Nilsson's avatar
Anders Nilsson committed
322
323
  int index)
{
324
  struct labcomm_sample_entry *p;
Anders Nilsson's avatar
Anders Nilsson committed
325
326
327
328
329
  for (p = head ; p && p->index != index ; p = p->next) {
  }
  return p;
}

330
static int get_local_index(
331
  struct labcomm_signature *s)
332
{
333
  int result = -ENOENT;
334
335
336

  if (&labcomm_first_signature <= s && s < &labcomm_last_signature) {
    result = (int)(s - &labcomm_first_signature) + LABCOMM_USER;
337
338
339
340
  }
  return result;
}

341
static int get_encoder_index(
342
343
  struct labcomm_encoder *e,
  struct labcomm_signature *s)
344
{
345
  return get_local_index(s);
346
347
}

348
struct labcomm_encoder *labcomm_encoder_new(
349
  struct labcomm_writer *writer,
350
  struct labcomm_lock *lock)
Anders Nilsson's avatar
Anders Nilsson committed
351
{
352
  struct labcomm_encoder *result = malloc(sizeof(*result));
Anders Nilsson's avatar
Anders Nilsson committed
353
  if (result) {
354
    struct labcomm_encoder_context *context;
Anders Nilsson's avatar
Anders Nilsson committed
355

356
    context = malloc(sizeof(*context));
357
#ifdef LABCOMM_ENCODER_LINEAR_SEARCH
Oscar Olsson's avatar
Oscar Olsson committed
358
    context->sample = NULL;
Anders Nilsson's avatar
Anders Nilsson committed
359
    context->index = LABCOMM_USER;
360
361
362
#else
    context->by_section = NULL;
#endif
Anders Nilsson's avatar
Anders Nilsson committed
363
    result->context = context;
364
    result->writer = writer;
365
    result->writer->data = NULL;
366
367
368
369
    result->writer->data_size = 0;
    result->writer->count = 0;
    result->writer->pos = 0;
    result->writer->error = 0;
370
    result->lock = lock;
371
372
373
374
    result->is_deferred = NULL;
    result->busy = 0;
    result->waiting = 0;
    result->alloc_action = NULL;
Oscar Olsson's avatar
Oscar Olsson committed
375
    result->on_error = on_error_fprintf;
376
    LABCOMM_SIGNATURE_ARRAY_INIT(result->registered, int);
Anders Nilsson's avatar
Anders Nilsson committed
377
378
379
380
  }
  return result;
}

381
382
383
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
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
static int encoder_enqueue_action(
  struct labcomm_encoder *encoder, 
  void (*action)(struct labcomm_encoder *encoder, void *context),
  void *context)
{
  int result;
  struct encoder_alloc_action *element, **next;

  fprintf(stderr, "%s %p\n", __FUNCTION__, action);
  element = malloc(sizeof(*action));
  if (element == NULL) {
    result = -ENOMEM;
    goto out;
  }
  element->next = NULL;
  element->action = action;
  element->context = context;
  for (next = &encoder->alloc_action ; *next ; next = &(*next)->next) {
  }
  *next = element;
  result = 0;
out:
  return result;
}

static struct labcomm_encoder *enter_encoder(struct labcomm_encoder *e)
{
  if (e->is_deferred) {
    return e->is_deferred;
  } else {
    labcomm_lock_acquire(e->lock); 
    e->waiting++;
    while (e->busy) { labcomm_lock_wait(e->lock, 10000000); }
    e->busy = 1;
    labcomm_lock_release(e->lock);
    
    if (e->writer->data == NULL) {
      labcomm_writer_alloc(e->writer,e->writer->action_context,
			   e, LABCOMM_VERSION, encoder_enqueue_action);
      if (e->alloc_action) {
	struct labcomm_encoder deferred;
	struct encoder_alloc_action *p;

	deferred.is_deferred = e;
	p = e->alloc_action;
	e->alloc_action = NULL;
	while (p) {
	  struct encoder_alloc_action *tmp;

	  fprintf(stderr, "RUN %p", p->action);
	  p->action(&deferred, p->context);
	  tmp = p;
	  p = p->next;
	  free(tmp);
	}
      }
    }
  }
  return e;
}
static void leave_encoder(struct labcomm_encoder *e)
{
  if (!e->is_deferred) {
    labcomm_lock_acquire(e->lock); {
      e->busy = 0;
      e->waiting--;
      if (e->waiting) {
	labcomm_lock_notify(e->lock);
      }
    } labcomm_lock_release(e->lock);
  }
}

int labcomm_internal_encoder_register(
  struct labcomm_encoder *encoder,
456
  struct labcomm_signature *signature,
457
  labcomm_encoder_function encode)
Anders Nilsson's avatar
Anders Nilsson committed
458
{
459
460
461
462
463
464
  int result = -EINVAL;
  struct labcomm_encoder *e;
  int index;

  e = enter_encoder(encoder);
  index = get_encoder_index(e, signature);
465
  if (signature->type == LABCOMM_SAMPLE) {
466
467
468
    if (index > 0) {
      int *registered = LABCOMM_SIGNATURE_ARRAY_REF(e->registered, int, index);
      if (! *registered) {
469
	int err;
470
	
471
	*registered = 1;	
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
	err = labcomm_writer_start(e->writer, e->writer->action_context, 
				   index, signature, NULL);
	if (err == -EALREADY) {
	  result = 0;
	} else if (err == 0) {
	  int i;

	  labcomm_write_packed32(e->writer, signature->type);
	  labcomm_write_packed32(e->writer, index);
	  labcomm_write_string(e->writer, signature->name);
	  for (i = 0 ; i < signature->size ; i++) {
	    if (e->writer->pos >= e->writer->count) {
	      labcomm_writer_flush(e->writer, e->writer->action_context);
	    }
	    e->writer->data[e->writer->pos] = signature->signature[i];
	    e->writer->pos++;
	  }
	  labcomm_writer_end(e->writer, e->writer->action_context);
	  result = e->writer->error;
491
492
493
	}
      }
    }
Anders Nilsson's avatar
Anders Nilsson committed
494
  }
495
496
  leave_encoder(encoder);
  return result;
Anders Nilsson's avatar
Anders Nilsson committed
497
498
}

499
int labcomm_internal_encode(
500
  struct labcomm_encoder *encoder,
501
  struct labcomm_signature *signature,
502
  labcomm_encoder_function encode,
Anders Nilsson's avatar
Anders Nilsson committed
503
504
  void *value)
{
505
  int result;
506
  struct labcomm_encoder *e;
507
508
  int index;

509
  e = enter_encoder(encoder);
510
  index = get_encoder_index(e, signature);
511
  result = labcomm_writer_start(e->writer, e->writer->action_context, 
512
				index, signature, value);
513
514
  if (result == -EALREADY) { result = 0; goto no_end; }
  if (result != 0) { goto out; }
515
  result = labcomm_write_packed32(e->writer, index);
516
  if (result != 0) { goto out; }
517
  result = encode(e->writer, value);
518
out:
519
  labcomm_writer_end(e->writer, e->writer->action_context);
520
no_end:
521
  leave_encoder(encoder);
522
  return result;
Anders Nilsson's avatar
Anders Nilsson committed
523
}
Riccardo Gaiati's avatar
Riccardo Gaiati committed
524

525
void labcomm_encoder_free(struct labcomm_encoder* e)
Anders Nilsson's avatar
Anders Nilsson committed
526
{
527
  struct labcomm_encoder_context *context;
Oscar Olsson's avatar
Oscar Olsson committed
528

529
  context = (struct labcomm_encoder_context *) e->context;
530
  labcomm_writer_free(e->writer, e->writer->action_context);
531
532
  LABCOMM_SIGNATURE_ARRAY_FREE(e->registered, int);

533
#ifdef LABCOMM_ENCODER_LINEAR_SEARCH
534
535
  struct labcomm_sample_entry *entry = context->sample;
  struct labcomm_sample_entry *entry_next;
536
537
538
539
540
541
542
543
  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
544
  free(e->context);
Anders Nilsson's avatar
Anders Nilsson committed
545
546
547
  free(e);
}

548
int labcomm_encoder_ioctl(struct labcomm_encoder *encoder, 
549
550
			  uint32_t action,
			  ...)
Anders Nilsson's avatar
Anders Nilsson committed
551
{
Anders Blomdell's avatar
Anders Blomdell committed
552
553
  int result;
  va_list va;
554
    
Anders Blomdell's avatar
Anders Blomdell committed
555
556
557
558
559
560
  if (LABCOMM_IOC_SIG(action) != LABCOMM_IOC_NOSIG) {
    result = -EINVAL;
    goto out;
  }
    
  va_start(va, action);
561
562
563
  result = labcomm_writer_ioctl(encoder->writer, 
			       encoder->writer->action_context,
			       0, NULL, action, va);
Anders Blomdell's avatar
Anders Blomdell committed
564
565
566
  va_end(va);

out:
567
  return result;
Anders Nilsson's avatar
Anders Nilsson committed
568
569
}

570
571
572
static int writer_ioctl(struct labcomm_writer *writer,
			uint32_t action,
			...)
573
{
Anders Blomdell's avatar
Anders Blomdell committed
574
575
576
577
578
579
580
581
582
  int result;
  va_list va;

  if (LABCOMM_IOC_SIG(action) != LABCOMM_IOC_NOSIG) {
    result = -EINVAL;
    goto out;
  }
  
  va_start(va, action);
583
584
  result = labcomm_writer_ioctl(writer, writer->action_context, 
				0, NULL, action, va);
Anders Blomdell's avatar
Anders Blomdell committed
585
586
  va_end(va);
out:
587
588
589
  return result;
}

590
int labcomm_internal_encoder_ioctl(struct labcomm_encoder *encoder, 
591
				   struct labcomm_signature *signature,
592
				   uint32_t action, va_list va)
593
594
595
{
  int result = -ENOTSUP;
  
596
597
598
  result = labcomm_writer_ioctl(encoder->writer, 
				encoder->writer->action_context, 
				-1, signature, action, va);
599
600
601
  return result;
}

Anders Nilsson's avatar
Anders Nilsson committed
602
static void collect_flat_signature(
603
  struct labcomm_decoder *decoder,
604
  struct labcomm_writer *writer)
Anders Nilsson's avatar
Anders Nilsson committed
605
{
606
  int type = labcomm_read_packed32(decoder->reader); 
Anders Nilsson's avatar
Anders Nilsson committed
607
  if (type >= LABCOMM_USER) {
608
609
    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
610
  } else {
611
    labcomm_write_packed32(writer, type); 
Anders Nilsson's avatar
Anders Nilsson committed
612
613
614
    switch (type) {
      case LABCOMM_ARRAY: {
	int dimensions, i;
Riccardo Gaiati's avatar
Riccardo Gaiati committed
615

616
	dimensions = labcomm_read_packed32(decoder->reader);
617
	labcomm_write_packed32(writer, dimensions);
Anders Nilsson's avatar
Anders Nilsson committed
618
	for (i = 0 ; i < dimensions ; i++) {
619
	  int n = labcomm_read_packed32(decoder->reader);
620
	  labcomm_write_packed32(writer, n);
Anders Nilsson's avatar
Anders Nilsson committed
621
	}
622
	collect_flat_signature(decoder, writer);
Anders Nilsson's avatar
Anders Nilsson committed
623
624
625
      } break;
      case LABCOMM_STRUCT: {
	int fields, i;
626

627
	fields = labcomm_read_packed32(decoder->reader); 
628
	labcomm_write_packed32(writer, fields); 
Anders Nilsson's avatar
Anders Nilsson committed
629
	for (i = 0 ; i < fields ; i++) {
630
	  char *name = labcomm_read_string(decoder->reader);
631
	  labcomm_write_string(writer, name);
Anders Nilsson's avatar
Anders Nilsson committed
632
	  free(name);
633
	  collect_flat_signature(decoder, writer);
Anders Nilsson's avatar
Anders Nilsson committed
634
635
636
637
638
639
640
641
642
643
644
645
	}
      } 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: {
646
647
        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
648
649
650
651
652
      } break;
    }
  }
}

653
struct labcomm_decoder *labcomm_decoder_new(
654
  struct labcomm_reader *reader,
655
  struct labcomm_lock *lock)
656
{
657
  struct labcomm_decoder *result = malloc(sizeof(*result));
658
  if (result) {
659
660
    struct labcomm_decoder_context *context =
      (struct labcomm_decoder_context *)malloc(sizeof(*context));
661
662
    context->sample = 0;
    result->context = context;
663
664
665
666
667
    result->reader = reader;
    result->reader->data = 0;
    result->reader->data_size = 0;
    result->reader->count = 0;
    result->reader->pos = 0;
Anders Blomdell's avatar
Anders Blomdell committed
668
    result->reader->error = 0;
669
    result->lock = lock;
670
671
    result->on_error = on_error_fprintf;
    result->on_new_datatype = on_new_datatype;
672
673
    LABCOMM_SIGNATURE_ARRAY_INIT(result->local_to_remote, int);
    LABCOMM_SIGNATURE_ARRAY_INIT(result->remote_to_local, int);
674
675
676
677
  }
  return result;
}

678
int labcomm_internal_decoder_register(
679
680
681
682
  struct labcomm_decoder *d,
  struct labcomm_signature *signature,
  labcomm_decoder_function type_decoder,
  labcomm_handler_function handler,
Anders Nilsson's avatar
Anders Nilsson committed
683
684
  void *handler_context)
{
685
686
  struct labcomm_decoder_context *context = d->context;
  struct labcomm_sample_entry *sample;
Riccardo Gaiati's avatar
Riccardo Gaiati committed
687
  sample = get_sample_by_signature_address(context->sample,
Anders Nilsson's avatar
Anders Nilsson committed
688
689
					   signature);
  if (!sample) {
690
    sample = (struct labcomm_sample_entry *)malloc(sizeof(*sample));
Anders Nilsson's avatar
Anders Nilsson committed
691
692
693
694
    sample->next = context->sample;
    context->sample = sample;
    sample->index = 0;
    sample->signature = signature;
Riccardo Gaiati's avatar
Riccardo Gaiati committed
695
  }
Anders Nilsson's avatar
Anders Nilsson committed
696
697
698
  sample->decoder = type_decoder;
  sample->handler = handler;
  sample->context = handler_context;
699
700

  return 0;
Anders Nilsson's avatar
Anders Nilsson committed
701
702
}

703
static int decode_typedef_or_sample(struct labcomm_decoder *d, int kind)
Anders Nilsson's avatar
Anders Nilsson committed
704
705
{
  int result;
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
  struct labcomm_decoder_context *context = d->context;

  /* TODO: should the labcomm_dynamic_buffer_writer be 
     a permanent part of labcomm_decoder? */
  struct labcomm_writer_action_context action_context = {
    .next = NULL,
    .action = labcomm_dynamic_buffer_writer_action,
    .context = NULL
  };
  struct labcomm_writer writer = {
    .action_context = &action_context,
    .data = NULL,
    .data_size = 0,
    .count = 0,
    .pos = 0,
    .error = 0,
  };
  struct labcomm_signature signature;
  struct labcomm_sample_entry *entry = NULL;
  int remote_index, err;
      
  labcomm_writer_alloc(&writer, writer.action_context, NULL, "", NULL);
  labcomm_writer_start(&writer, writer.action_context, 0, NULL, NULL);
  remote_index = labcomm_read_packed32(d->reader); //int
  signature.name = labcomm_read_string(d->reader);
  signature.type = kind;
  collect_flat_signature(d, &writer);
  labcomm_writer_end(&writer, writer.action_context);
  err = writer_ioctl(&writer, 
		     LABCOMM_IOCTL_WRITER_GET_BYTES_WRITTEN,
		     &signature.size);
  if (err < 0) {
    printf("Failed to get size: %s\n", strerror(-err));
    result = -ENOENT;
    goto free_signature_name;
  }
  err = writer_ioctl(&writer, 
		     LABCOMM_IOCTL_WRITER_GET_BYTE_POINTER,
		     &signature.signature);
  if (err < 0) {
    printf("Failed to get pointer: %s\n", strerror(-err));
    result = -ENOENT;
    goto free_signature_name;
  }
  entry = get_sample_by_signature_value(context->sample, &signature);
  if (! entry) {
    fprintf(stderr, "%d %s\n", remote_index, signature.name);
    /* Unknown datatype, bail out */
    d->on_new_datatype(d, &signature);
    result = -ENOENT;
  } else if (entry->index && entry->index != remote_index) {
    d->on_error(LABCOMM_ERROR_DEC_INDEX_MISMATCH, 5, 
		"%s(): index mismatch '%s' (id=0x%x != 0x%x)\n", 
		__FUNCTION__, signature.name, entry->index, remote_index);
    result = -ENOENT;
  } else {
    int local_index;
    int *local_to_remote, *remote_to_local;
    // TODO unnessesary, since entry->index == index in above if statement
    entry->index = remote_index;
    local_index = get_local_index(entry->signature);
    local_to_remote = LABCOMM_SIGNATURE_ARRAY_REF(d->local_to_remote, int,
						  local_index);
    *local_to_remote = remote_index;
    remote_to_local = LABCOMM_SIGNATURE_ARRAY_REF(d->remote_to_local, int,
						  remote_index);
    *remote_to_local = local_index;
    result = remote_index;
    labcomm_reader_start(d->reader, d->reader->action_context,
			 entry->index, entry->signature,
			 NULL);
    labcomm_reader_end(d->reader, d->reader->action_context);
  }
free_signature_name:
  free(signature.name);
  labcomm_writer_free(&writer, writer.action_context);
  if (!entry) {
    // No handler for found type, bail out (after cleanup)
    result = -ENOENT;
  }
  return result;
}

struct call_handler_context {
  struct labcomm_sample_entry *entry;
  struct labcomm_reader *reader;
};

static void call_handler(void *value, void *context)
{
  struct call_handler_context *wrap = context;

  labcomm_reader_start(wrap->reader, wrap->reader->action_context,
		       wrap->entry->index, wrap->entry->signature,
		       value);
  wrap->entry->handler(value, wrap->entry->context);
  labcomm_reader_end(wrap->reader, wrap->reader->action_context);
}

int labcomm_decoder_decode_one(struct labcomm_decoder *d)
{
  int result, index;
  struct labcomm_decoder_context *context = d->context;
809
810
  
  if (d->reader->data == NULL) {
811
812
    result = labcomm_reader_alloc(d->reader, d->reader->action_context,
				  d, LABCOMM_VERSION);
813
814
815
816
    if (result <= 0) {
      goto out;
    }
  }
817
818
819
820
821
822
823
824
825
  index = labcomm_read_packed32(d->reader);
  if (d->reader->error < 0) {
    result = d->reader->error;
    goto out;
  }
  if (index == LABCOMM_TYPEDEF || index == LABCOMM_SAMPLE) {
    result = decode_typedef_or_sample(d, index); 
  } else {
    struct labcomm_sample_entry *entry;
826
    
827
828
829
830
831
832
833
834
    entry = get_sample_by_index(context->sample, index);
    if (!entry) {
      // 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__, index);
      result = -ENOENT;
835
    } else {
836
837
838
839
840
841
      struct call_handler_context wrap = {
	.entry = entry,
	.reader = d->reader
      };
      entry->decoder(d->reader, call_handler, &wrap);
      result = index;
Anders Nilsson's avatar
Anders Nilsson committed
842
    }
843
  }
844
out:
Anders Nilsson's avatar
Anders Nilsson committed
845
846
847
  return result;
}

848
void labcomm_decoder_run(struct labcomm_decoder *d)
Anders Nilsson's avatar
Anders Nilsson committed
849
850
851
852
853
{
  while (labcomm_decoder_decode_one(d) > 0) {
  }
}

854
void labcomm_decoder_free(struct labcomm_decoder* d)
Anders Nilsson's avatar
Anders Nilsson committed
855
{
856
857
858
  struct labcomm_decoder_context *context = (struct labcomm_decoder_context *) d->context;
  struct labcomm_sample_entry *entry = context->sample;
  struct labcomm_sample_entry *entry_next;
Oscar Olsson's avatar
Oscar Olsson committed
859

860
  labcomm_reader_free(d->reader, d->reader->action_context);
861
862
  LABCOMM_SIGNATURE_ARRAY_FREE(d->local_to_remote, int);
  LABCOMM_SIGNATURE_ARRAY_FREE(d->remote_to_local, int);
Oscar Olsson's avatar
Oscar Olsson committed
863
864
865
866
867
868
869
  while (entry != NULL) {
    entry_next = entry->next;
    free(entry);
    entry = entry_next;
  }

  free(d->context);
Anders Nilsson's avatar
Anders Nilsson committed
870
871
  free(d);
}
872
873

int labcomm_decoder_ioctl(struct labcomm_decoder *decoder, 
874
			  uint32_t action,
875
876
			  ...)
{
877
878
  int result;  
  va_list va;
879
    
880
  va_start(va, action);
881
882
883
  result = labcomm_reader_ioctl(decoder->reader, 
				decoder->reader->action_context,
				0, NULL, action, va);
884
  va_end(va);
885
886
887
888
  return result;
}

int labcomm_internal_decoder_ioctl(struct labcomm_decoder *decoder, 
889
				   struct labcomm_signature *signature,
890
				   uint32_t action, va_list va)
891
{
892
893
894
895
896
897
898
899
900
  int result;
  int local_index, *remote_index;

  local_index = get_local_index(signature);
  remote_index = LABCOMM_SIGNATURE_ARRAY_REF(decoder->local_to_remote, int,
					     local_index);
  if (*remote_index == 0) {
    result = -EAGAIN;
  } else {
901
902
903
904
    result = labcomm_reader_ioctl(decoder->reader, 
				  decoder->reader->action_context,
				  *remote_index, signature, 
				  action, va);
905
906
907
908
  }
  return result;
}

909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
#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

925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
void *labcomm_signature_array_ref(int *first, int *last, void **data,
				  int size, int index)
{
  if (*first == 0 && *last == 0) {
    *first = index;
    *last = index + 1;
    *data = malloc(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 = malloc(n * size);
    if (*data) {
      memset(*data, 0, n * size);
      memcpy(*data + (old_first - *first) * size, 
	     old_data, 
	     (old_last - old_first) * size);
    }
950
//    dump(old_data, size, old_first, old_last);
951
952
953
    free(old_data);
  }
  if (*data) {
954
//    dump(*data, size, *first, *last);
955
956
957
958
959
    return *data + (index - *first) * size;
  } else {
    return NULL;
  }
}