labcomm_decoder.c 12.5 KB
Newer Older
Anders Blomdell's avatar
Anders Blomdell committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
/*
  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/>.
*/
21
#define CURRENT_VERSION "LabComm20141009"
Anders Blomdell's avatar
Anders Blomdell committed
22
23
24
25
26
27

#include <errno.h>
#include "labcomm.h"
#include "labcomm_private.h"
#include "labcomm_ioctl.h"
#include "labcomm_dynamic_buffer_writer.h"
28
#include "labcomm_bytearray_reader.h"
Anders Blomdell's avatar
Anders Blomdell committed
29
30
31
32
33
34
35
36
37
38
39

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;
40
  int reader_allocated;
41
  int version_ok;
Anders Blomdell's avatar
Anders Blomdell committed
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
  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;
68
    result->reader_allocated = 0;
69
    result->version_ok = 0;
Anders Blomdell's avatar
Anders Blomdell committed
70
71
72
    result->error = error;
    result->memory = memory;
    result->scheduler = scheduler;
73
    result->on_error = on_error_fprintf;
Anders Blomdell's avatar
Anders Blomdell committed
74
75
76
77
78
    LABCOMM_SIGNATURE_ARRAY_INIT(result->local, struct sample_entry);
    LABCOMM_SIGNATURE_ARRAY_INIT(result->remote_to_local, int);
  }
  return result;
}
79
80
81
82
83
84
85
86
87
88
89
90
#ifndef WITHOUT_PRAGMA
/* Internal aux function to allow temporary internal decoders
 * to share the same memory.
 * This function only frees the decoder, not the memory
 */
static void internal_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);
}
Anders Blomdell's avatar
Anders Blomdell committed
91

92
93
94
95
96
97
98
99
void labcomm_decoder_free(struct labcomm_decoder* d)
{
  struct labcomm_memory *memory = d->memory;
  internal_labcomm_decoder_free(d);
  labcomm_memory_free(memory, 0, d);
}

#else
Anders Blomdell's avatar
Anders Blomdell committed
100
101
102
103
104
105
106
107
108
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);
}
109
#endif
Anders Blomdell's avatar
Anders Blomdell committed
110

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
111
static int decode_sample_def(struct labcomm_decoder *d, int kind)
Anders Blomdell's avatar
Anders Blomdell committed
112
113
{
  int result;
114
  struct labcomm_signature signature, *local_signature;
Anders Blomdell's avatar
Anders Blomdell committed
115
  int remote_index, local_index, i;
116
117
118
  
  local_signature = NULL;
  local_index = 0;
119
  remote_index = labcomm_read_packed32(d->reader);
Anders Blomdell's avatar
Anders Blomdell committed
120
121
122
123
  if (d->reader->error < 0) {
    result = d->reader->error;
    goto out;
  }
Anders Blomdell's avatar
Anders Blomdell committed
124
  signature.name = labcomm_read_string(d->reader);
Anders Blomdell's avatar
Anders Blomdell committed
125
126
127
  if (d->reader->error < 0) {
    result = d->reader->error;
    goto out;
Anders Blomdell's avatar
Anders Blomdell committed
128
  }
Anders Blomdell's avatar
Anders Blomdell committed
129
130
131
  signature.size = labcomm_read_packed32(d->reader);
  if (d->reader->error < 0) {
    result = d->reader->error;
132
    goto free_signature_name;
Anders Blomdell's avatar
Anders Blomdell committed
133
134
135
136
  }
  signature.signature = labcomm_memory_alloc(d->memory, 1,  signature.size);
  if (d->reader->error < 0) {
    result = d->reader->error;
Anders Blomdell's avatar
Anders Blomdell committed
137
138
    goto free_signature_name;
  }
Anders Blomdell's avatar
Anders Blomdell committed
139
140
141
142
143
144
145
146
147
148
149
  for (i = 0 ; i < signature.size ; i++) {
    signature.signature[i] = labcomm_read_byte(d->reader);
    if (d->reader->error < 0) {
      result = d->reader->error;
      goto free_signature_signature;
    }
  }
  labcomm_scheduler_data_lock(d->scheduler);
  LABCOMM_SIGNATURE_ARRAY_FOREACH(d->local, struct sample_entry, i) {
    struct sample_entry *s;
    int *remote_to_local;
Anders Blomdell's avatar
Anders Blomdell committed
150
      
Anders Blomdell's avatar
Anders Blomdell committed
151
152
    result = -ENOENT;
    s = LABCOMM_SIGNATURE_ARRAY_REF(d->memory, 
Anders Blomdell's avatar
Anders Blomdell committed
153
				      d->local,  struct sample_entry, i);
Anders Blomdell's avatar
Anders Blomdell committed
154
155
156
157
    if (s->signature &&
        s->signature->size == signature.size &&
        strcmp(s->signature->name, signature.name) == 0 &&
        memcmp((void*)s->signature->signature, (void*)signature.signature,
Anders Blomdell's avatar
Anders Blomdell committed
158
	       signature.size) == 0) {
Anders Blomdell's avatar
Anders Blomdell committed
159
160
161
162
163
164
165
166
167
      s->remote_index = remote_index;
      local_signature = s->signature;
      local_index = i;
      remote_to_local = LABCOMM_SIGNATURE_ARRAY_REF(d->memory,
                                                    d->remote_to_local, int,
                                                    remote_index);
      *remote_to_local = i;
      result = remote_index;
      break;
168
    }
Anders Blomdell's avatar
Anders Blomdell committed
169
  }
Anders Blomdell's avatar
Anders Blomdell committed
170
171
172
173
174
175
176
177
178
  labcomm_scheduler_data_unlock(d->scheduler);
  if (local_signature) {
    labcomm_reader_start(d->reader, d->reader->action_context,
                         local_index, remote_index, local_signature,
                         NULL);
    labcomm_reader_end(d->reader, d->reader->action_context);
  }
free_signature_signature:
  labcomm_memory_free(d->memory, 1,  signature.signature);
Anders Blomdell's avatar
Anders Blomdell committed
179
180
free_signature_name:
  labcomm_memory_free(d->memory, 0, signature.name);
Anders Blomdell's avatar
Anders Blomdell committed
181
out:
Anders Blomdell's avatar
Anders Blomdell committed
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
  return result;
}

struct call_handler_context {
  struct labcomm_reader *reader;
  int local_index;
  int remote_index;
  struct labcomm_signature *signature;
  labcomm_handler_function handler;
  void *context;
};

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

198
199
200
201
202
203
204
  if (wrap->reader->error >= 0) {
    labcomm_reader_start(wrap->reader, wrap->reader->action_context,
			 wrap->local_index, wrap->remote_index, wrap->signature,
			 value);
    wrap->handler(value, wrap->context);
    labcomm_reader_end(wrap->reader, wrap->reader->action_context);
  }
Anders Blomdell's avatar
Anders Blomdell committed
205
206
}

207
static void reader_alloc(struct labcomm_decoder *d)
Anders Blomdell's avatar
Anders Blomdell committed
208
{
209
210
  if (!d->reader_allocated) {
    d->reader_allocated = 1;
211
    labcomm_reader_alloc(d->reader, d->reader->action_context);
Anders Blomdell's avatar
Anders Blomdell committed
212
213
  }
}
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
static int decode_version(struct labcomm_decoder *d, int remote_index)
{
    int result;
    char *version = labcomm_read_string(d->reader);
    if (d->reader->error < 0) {
      result = d->reader->error;
      goto out;
    }
    if (strcmp(version, CURRENT_VERSION) == 0) {
      result = LABCOMM_VERSION;
      d->version_ok = 1;
    } else {
      result = -ECONNRESET;
    }  
    labcomm_memory_free(d->memory, 1,  version);
out:   
  return result;
}
232
233
234
235

static int decoder_skip(struct labcomm_decoder *d, int len)
{
  int i;
236
  printf("skipping %d bytes\n", len);
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
  for(i = 0; i <len; i++){
    labcomm_read_byte(d->reader);
    if (d->reader->error < 0) {
      return d->reader->error;
    }
  }
  return 0;
}
static int decode_pragma(struct labcomm_decoder *d, int len)
{
  char *pragma_type;
  int result;
  pragma_type = labcomm_read_string(d->reader);
  if (d->reader->error < 0) {
    result = d->reader->error;
    goto out;
  }
  int bytes = labcomm_size_string(pragma_type);
  int psize = len-bytes;
256
  printf("got pragma: %s\n", pragma_type);
257
258
259
260
261
262
263
264
265
266
267
268
269
  if(0 /*d->pragma_handler*/) {
  //read the entire packet to a buffer and then run
  // decode on that through a bytearray_reader.
  // (to easily handle multiple labcomm packets in one metadata packet)
    int i;
    unsigned char pragma_data[psize] ;
    for(i=0; i<psize; i++) {
       pragma_data[i] = labcomm_read_byte(d->reader);
       if (d->reader->error < 0) {
         result = d->reader->error;
         goto out;
       }
     }
270
271
272
    printf("read %d bytes\n", psize);
    //TODO: add wrapped decoders, and create pragma decoder that does not
    //      expect a version packet
273
274
275
276
277
     struct labcomm_reader *pr = labcomm_bytearray_reader_new(
                                     d->reader->memory, pragma_data, psize);
     struct labcomm_decoder *pd = labcomm_decoder_new(
                                     pr, d->error, d->memory, d->scheduler);
    /* d->prama_handler(pd, ptype, plen); */
278
     labcomm_decoder_run(pd);
279
280
281
282
283
284
285
286
287
288

     internal_labcomm_decoder_free(pd);
     result = 0;
   } else {
     result = decoder_skip(d, psize);
   }
out:
  return result;
}

Anders Blomdell's avatar
Anders Blomdell committed
289
290
int labcomm_decoder_decode_one(struct labcomm_decoder *d)
{
291
  int result, remote_index, length;
Anders Blomdell's avatar
Anders Blomdell committed
292

293
  reader_alloc(d);
Anders Blomdell's avatar
Anders Blomdell committed
294
295
296
297
298
  remote_index = labcomm_read_packed32(d->reader);
  if (d->reader->error < 0) {
    result = d->reader->error;
    goto out;
  }
299
300
301
302
303
  length = labcomm_read_packed32(d->reader);
  if (d->reader->error < 0) {
    result = d->reader->error;
    goto out;
  }
304
  if (remote_index == LABCOMM_VERSION) {
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
305
    result = decode_version(d, remote_index);
306
307
308
  } else if (! d->version_ok) {
    fprintf(stderr, "No VERSION %d %d\n", remote_index, length);
    result = -ECONNRESET;
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
309
310
  } else if (remote_index == LABCOMM_SAMPLE_DEF) {
    result = decode_sample_def(d, remote_index); 
311
312
  } else if (remote_index == LABCOMM_PRAGMA ){
    result = decode_pragma(d, length);
313
314
315
  } else if (remote_index < LABCOMM_USER) {
    fprintf(stderr, "SKIP %d %d\n", remote_index, length);
    result = remote_index;
Anders Blomdell's avatar
Anders Blomdell committed
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
  } else {
    int *local_index;
    struct call_handler_context wrap = {
      .reader = d->reader,
      .remote_index = remote_index,
      .signature = NULL,
      .handler = NULL,
      .context = NULL,
    };
    labcomm_decoder_function do_decode = NULL;

    labcomm_scheduler_data_lock(d->scheduler);
    local_index = LABCOMM_SIGNATURE_ARRAY_REF(d->memory,
					      d->remote_to_local, int,
					      remote_index);
    if (*local_index != 0) {
      struct sample_entry *entry;

      entry = LABCOMM_SIGNATURE_ARRAY_REF(d->memory,
					  d->local, struct sample_entry,
					  *local_index);
      wrap.local_index = *local_index;
      wrap.signature = entry->signature;
      wrap.handler = entry->handler;
      wrap.context = entry->context;
      do_decode = entry->decode;
      result = *local_index;
    }
    labcomm_scheduler_data_unlock(d->scheduler);
    if (do_decode) {
      do_decode(d->reader, call_handler, &wrap);
347
348
349
      if (d->reader->error < 0) {
	result = d->reader->error;
      }
Anders Blomdell's avatar
Anders Blomdell committed
350
351
352
353
    } else {
      result = -ENOENT;
    }
  }
354
out:   
Anders Blomdell's avatar
Anders Blomdell committed
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
  return result;
}

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

int labcomm_decoder_ioctl(struct labcomm_decoder *d, 
			  uint32_t action,
			  ...)
{
  int result;  
  va_list va;
    
  va_start(va, action);
  result = labcomm_reader_ioctl(d->reader, 
				d->reader->action_context,
374
				0, 0, NULL, action, va);
Anders Blomdell's avatar
Anders Blomdell committed
375
376
377
378
379
380
381
382
383
384
385
  va_end(va);
  return result;
}

int labcomm_internal_decoder_ioctl(struct labcomm_decoder *d, 
				   struct labcomm_signature *signature,
				   uint32_t action, va_list va)
{
  int result;
  int local_index, remote_index;

386
  local_index = labcomm_get_local_index(signature);
Anders Blomdell's avatar
Anders Blomdell committed
387
388
389
390
391
392
  labcomm_scheduler_data_lock(d->scheduler);
  remote_index = LABCOMM_SIGNATURE_ARRAY_REF(d->memory,
					     d->local,
					     struct sample_entry,
					     local_index)->remote_index;
  labcomm_scheduler_data_unlock(d->scheduler);
393
394
395
  result = labcomm_reader_ioctl(d->reader, d->reader->action_context,
				local_index, remote_index, 
				signature, action, va);
Anders Blomdell's avatar
Anders Blomdell committed
396
397
398
399
400
401
402
403
404
405
406
407
408
  return result;
}

int labcomm_internal_decoder_register(
  struct labcomm_decoder *d,
  struct labcomm_signature *signature,
  labcomm_decoder_function decode, 
  labcomm_handler_function handler,
  void *context)
{
  int local_index;
  struct sample_entry *entry;
 
409
  reader_alloc(d);
410
  local_index = labcomm_get_local_index(signature);
Anders Blomdell's avatar
Anders Blomdell committed
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
  if (local_index <= 0) { goto out; }
  labcomm_reader_start(d->reader, d->reader->action_context,
		       local_index, 0, signature,
		       NULL);
  labcomm_reader_end(d->reader, d->reader->action_context);

  labcomm_scheduler_data_lock(d->scheduler);
  entry = LABCOMM_SIGNATURE_ARRAY_REF(d->memory,
				      d->local, struct sample_entry,
				      local_index);
  if (entry == NULL) { local_index = -ENOMEM; goto unlock; }
  entry->remote_index = 0;
  entry->signature = signature;
  entry->decode = decode;
  entry->handler = handler;
  entry->context = context;
unlock:
  labcomm_scheduler_data_unlock(d->scheduler);
out:
  return local_index;
}