labcomm_decoder.c 15.8 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 "LabComm2014"
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

struct sample_entry {
  int remote_index;
  struct labcomm_signature *signature;
  labcomm_decoder_function decode;
  labcomm_handler_function handler;
  void *context;
};

38
static int internal_decoder_run(struct labcomm_decoder *d,
39
		                struct labcomm_decoder *registry);
40
static int internal_decode_one(struct labcomm_decoder *d,
41
		               struct labcomm_decoder *registry);
42
int default_pragma_handler(struct labcomm_decoder *d,
43
		           struct labcomm_decoder *registry,
44 45
		           char *pragma_type,
			   void *context)
46 47
{
  printf("Default pragma handler got pragma: %s\n", pragma_type);
48
  int res = internal_decoder_run(d, registry);
49 50 51 52
  printf("... %d:%s\n",res,strerror(-res));
  return LABCOMM_PRAGMA;
}

53 54 55 56 57 58 59
labcomm_pragma_handler_callback default_pragma_handler_lookup(
                                        char *pragma_type,
                                        struct labcomm_decoder *d,
                                        void *context)
{
	return default_pragma_handler;
}
60

Anders Blomdell's avatar
Anders Blomdell committed
61 62
struct labcomm_decoder {
  struct labcomm_reader *reader;
63
  int reader_allocated;
64
  int version_ok;
Anders Blomdell's avatar
Anders Blomdell committed
65 66 67 68 69 70 71
  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);
72
  labcomm_pragma_handler_lookup pragma_handler_lookup;
Anders Blomdell's avatar
Anders Blomdell committed
73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91
};

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;
92
    result->reader_allocated = 0;
93
    result->version_ok = 0;
Anders Blomdell's avatar
Anders Blomdell committed
94 95 96
    result->error = error;
    result->memory = memory;
    result->scheduler = scheduler;
97
    result->on_error = on_error_fprintf;
Anders Blomdell's avatar
Anders Blomdell committed
98 99
    LABCOMM_SIGNATURE_ARRAY_INIT(result->local, struct sample_entry);
    LABCOMM_SIGNATURE_ARRAY_INIT(result->remote_to_local, int);
100
    result->pragma_handler_lookup = default_pragma_handler_lookup;
Anders Blomdell's avatar
Anders Blomdell committed
101 102 103
  }
  return result;
}
104 105 106 107 108 109 110 111 112 113 114 115
#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
116

117 118 119 120 121 122 123 124
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
125 126 127 128 129 130 131 132 133
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);
}
134
#endif
Anders Blomdell's avatar
Anders Blomdell committed
135

136 137 138 139 140 141
/* d        - decoder to read from
   registry - decoder to lookup signatures (registry != d only if
                nesting decoders, e.g., when decoding pragma)
   kind     - type to decode (sample 0x02, or TODO: typedef/binding )
*/
static int decode_sample_def(struct labcomm_decoder *d,
142 143
		             struct labcomm_decoder *registry,
			     int kind)
Anders Blomdell's avatar
Anders Blomdell committed
144 145
{
  int result;
146
  struct labcomm_signature signature, *local_signature;
Anders Blomdell's avatar
Anders Blomdell committed
147
  int remote_index, local_index, i;
148

149 150
  local_signature = NULL;
  local_index = 0;
151
  remote_index = labcomm_read_packed32(d->reader);
Anders Blomdell's avatar
Anders Blomdell committed
152 153 154 155
  if (d->reader->error < 0) {
    result = d->reader->error;
    goto out;
  }
Anders Blomdell's avatar
Anders Blomdell committed
156
  signature.name = labcomm_read_string(d->reader);
Anders Blomdell's avatar
Anders Blomdell committed
157 158 159
  if (d->reader->error < 0) {
    result = d->reader->error;
    goto out;
Anders Blomdell's avatar
Anders Blomdell committed
160
  }
Anders Blomdell's avatar
Anders Blomdell committed
161 162 163
  signature.size = labcomm_read_packed32(d->reader);
  if (d->reader->error < 0) {
    result = d->reader->error;
164
    goto free_signature_name;
Anders Blomdell's avatar
Anders Blomdell committed
165 166
  }
  signature.signature = labcomm_memory_alloc(d->memory, 1,  signature.size);
167
  //XXX d->reader???
Anders Blomdell's avatar
Anders Blomdell committed
168 169
  if (d->reader->error < 0) {
    result = d->reader->error;
Anders Blomdell's avatar
Anders Blomdell committed
170 171
    goto free_signature_name;
  }
Anders Blomdell's avatar
Anders Blomdell committed
172 173 174 175 176 177 178
  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;
    }
  }
179 180
  labcomm_scheduler_data_lock(registry->scheduler);
  LABCOMM_SIGNATURE_ARRAY_FOREACH(registry->local, struct sample_entry, i) {
Anders Blomdell's avatar
Anders Blomdell committed
181 182
    struct sample_entry *s;
    int *remote_to_local;
183

Anders Blomdell's avatar
Anders Blomdell committed
184
    result = -ENOENT;
185 186
    s = LABCOMM_SIGNATURE_ARRAY_REF(registry->memory,
				      registry->local,  struct sample_entry, i);
Anders Blomdell's avatar
Anders Blomdell committed
187 188 189 190
    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
191
	       signature.size) == 0) {
Anders Blomdell's avatar
Anders Blomdell committed
192 193 194
      s->remote_index = remote_index;
      local_signature = s->signature;
      local_index = i;
195 196
      remote_to_local = LABCOMM_SIGNATURE_ARRAY_REF(registry->memory,
                                                    registry->remote_to_local, int,
Anders Blomdell's avatar
Anders Blomdell committed
197 198 199 200
                                                    remote_index);
      *remote_to_local = i;
      result = remote_index;
      break;
201
    }
Anders Blomdell's avatar
Anders Blomdell committed
202
  }
203
  labcomm_scheduler_data_unlock(registry->scheduler);
Anders Blomdell's avatar
Anders Blomdell committed
204 205 206 207 208 209 210 211
  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
212 213
free_signature_name:
  labcomm_memory_free(d->memory, 0, signature.name);
Anders Blomdell's avatar
Anders Blomdell committed
214
out:
Anders Blomdell's avatar
Anders Blomdell committed
215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230
  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;

231 232 233 234 235 236 237
  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
238 239
}

240
static void reader_alloc(struct labcomm_decoder *d)
Anders Blomdell's avatar
Anders Blomdell committed
241
{
242 243
  if (!d->reader_allocated) {
    d->reader_allocated = 1;
244
    labcomm_reader_alloc(d->reader, d->reader->action_context);
Anders Blomdell's avatar
Anders Blomdell committed
245 246
  }
}
247 248
static int decode_version(struct labcomm_decoder *d,
                          int remote_index)
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
249 250 251 252 253 254 255 256 257 258 259 260
{
    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;
261
    }
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
262
    labcomm_memory_free(d->memory, 1,  version);
263
out:
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
264 265
  return result;
}
266

267
static int decoder_skip(struct labcomm_decoder *d, int len, int tag)
268 269
{
  int i;
270
  printf("got tag 0x%x, skipping %d bytes\n", tag, len);
271 272 273 274 275 276
  for(i = 0; i <len; i++){
    labcomm_read_byte(d->reader);
    if (d->reader->error < 0) {
      return d->reader->error;
    }
  }
277
  return tag;
278
}
279 280 281 282 283
/* d        - decoder to read from
   registry - decoder to lookup signatures (registry != d only if
                nesting decoders, e.g., when decoding pragma)
   len      - length of the labcomm packet )
*/
284 285 286
static int decode_pragma(struct labcomm_decoder *d,
		         struct labcomm_decoder *registry,
		         int len)
287 288 289
{
  char *pragma_type;
  int result;
290
  labcomm_pragma_handler_callback handler;
291 292 293 294 295 296 297
  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;
298 299
  handler = d->pragma_handler_lookup(pragma_type, d, 0);
  if(handler) {
300 301 302 303 304 305 306 307 308 309 310 311
  //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;
       }
     }
312 313 314 315 316
    //TODO: add wrapped decoders, by adding a parameter
    //struct labcomm_decoder *wrapped to all _internal_decode... functions
    //incl internal_decode_one()
    //and let the public function ...decode(d) call
    //_internal_decode(d,d)
317 318 319 320
     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);
321
     pd->version_ok = 1;
322
     void *pragma_context = 0;
323
     printf("calling pragma_handler\n");
324
     result = handler(pd, registry, pragma_type,pragma_context);
325
     printf("returned from pragma_handler\n");
326 327 328

     internal_labcomm_decoder_free(pd);
   } else {
329
     result = decoder_skip(d, psize, LABCOMM_PRAGMA);
330 331 332 333 334
   }
out:
  return result;
}

335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359
static labcomm_decoder_function lookup_h(struct labcomm_decoder *d,
		                         struct call_handler_context *wrap,
		                         int remote_index,
					 int **local_index)
{
  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;
  }
  labcomm_scheduler_data_unlock(d->scheduler);
  return do_decode;
}
360 361 362 363 364 365
/* d            - decoder to read from
   registry     - decoder to lookup signatures (registry != d only if
                    nesting decoders, e.g., when decoding pragma)
   remote_index -  received type index )
*/
static int decode_and_handle(struct labcomm_decoder *d,
366 367
		             struct labcomm_decoder *registry,
		             int remote_index)
368 369 370 371 372 373 374 375 376 377
{
  int result;
  int *local_index;
  struct call_handler_context wrap = {
    .reader = d->reader,
    .remote_index = remote_index,
    .signature = NULL,
    .handler = NULL,
    .context = NULL,
  };
378
  labcomm_decoder_function do_decode = lookup_h(registry, &wrap, remote_index, &local_index);
379 380 381 382 383 384 385 386 387 388 389
  result = *local_index;
  if (do_decode) {
    do_decode(d->reader, call_handler, &wrap);
    if (d->reader->error < 0) {
      result = d->reader->error;
    }
  } else {
    result = -ENOENT;
  }
  return result;
}
390
int labcomm_decoder_decode_one(struct labcomm_decoder *d)
391 392 393
{
	return internal_decode_one(d,d);
}
394 395 396 397
/* d            - decoder to read from
   registry     - decoder to lookup signatures (registry != d only if
                    nesting decoders, e.g., when decoding pragma)
*/
398 399
int internal_decode_one(struct labcomm_decoder *d,
		        struct labcomm_decoder *registry)
Anders Blomdell's avatar
Anders Blomdell committed
400
{
401
  int result, remote_index, length;
Anders Blomdell's avatar
Anders Blomdell committed
402

403
  reader_alloc(d);
Anders Blomdell's avatar
Anders Blomdell committed
404 405 406 407 408
  remote_index = labcomm_read_packed32(d->reader);
  if (d->reader->error < 0) {
    result = d->reader->error;
    goto out;
  }
409 410 411 412 413
  length = labcomm_read_packed32(d->reader);
  if (d->reader->error < 0) {
    result = d->reader->error;
    goto out;
  }
414
  if (remote_index == LABCOMM_VERSION) {
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
415
    result = decode_version(d, remote_index);
416 417 418
  } 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
419
  } else if (remote_index == LABCOMM_SAMPLE_DEF) {
420
    result = decode_sample_def(d, registry, remote_index);
421
  } else if (remote_index == LABCOMM_PRAGMA ){
422
    result = decode_pragma(d, registry, length);
423
  } else if (remote_index < LABCOMM_USER) {
424 425 426
    //fprintf(stderr, "SKIP %d %d\n", remote_index, length);
    //result = remote_index;
    result = decoder_skip(d, length, remote_index);
Anders Blomdell's avatar
Anders Blomdell committed
427
  } else {
428
    result = decode_and_handle(d, registry, remote_index);
Anders Blomdell's avatar
Anders Blomdell committed
429
  }
430
out:
Anders Blomdell's avatar
Anders Blomdell committed
431 432 433
  return result;
}

434
int labcomm_decoder_run(struct labcomm_decoder *d)
435 436 437
{
	return internal_decoder_run(d,d);
}
438 439 440 441 442
/* d            - decoder to read from
   registry     - decoder to lookup signatures (registry != d only if
                    nesting decoders, e.g., when decoding pragma)
*/
int internal_decoder_run(struct labcomm_decoder *d,
443
		         struct labcomm_decoder *registry)
Anders Blomdell's avatar
Anders Blomdell committed
444
{
445 446
  int res;
  while ( (res=internal_decode_one(d, registry)) > 0) {
Anders Blomdell's avatar
Anders Blomdell committed
447
  }
448
  return res;
Anders Blomdell's avatar
Anders Blomdell committed
449 450
}

451
int labcomm_decoder_ioctl(struct labcomm_decoder *d,
Anders Blomdell's avatar
Anders Blomdell committed
452 453 454
			  uint32_t action,
			  ...)
{
455
  int result;
Anders Blomdell's avatar
Anders Blomdell committed
456
  va_list va;
457

Anders Blomdell's avatar
Anders Blomdell committed
458
  va_start(va, action);
459
  result = labcomm_reader_ioctl(d->reader,
Anders Blomdell's avatar
Anders Blomdell committed
460
				d->reader->action_context,
461
				0, 0, NULL, action, va);
Anders Blomdell's avatar
Anders Blomdell committed
462 463 464 465
  va_end(va);
  return result;
}

466
int labcomm_internal_decoder_ioctl(struct labcomm_decoder *d,
Anders Blomdell's avatar
Anders Blomdell committed
467 468 469 470 471 472
				   struct labcomm_signature *signature,
				   uint32_t action, va_list va)
{
  int result;
  int local_index, remote_index;

473
  local_index = labcomm_get_local_index(signature);
Anders Blomdell's avatar
Anders Blomdell committed
474 475 476 477 478 479
  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);
480
  result = labcomm_reader_ioctl(d->reader, d->reader->action_context,
481
				local_index, remote_index,
482
				signature, action, va);
Anders Blomdell's avatar
Anders Blomdell committed
483 484 485 486 487 488
  return result;
}

int labcomm_internal_decoder_register(
  struct labcomm_decoder *d,
  struct labcomm_signature *signature,
489
  labcomm_decoder_function decode,
Anders Blomdell's avatar
Anders Blomdell committed
490 491 492 493 494
  labcomm_handler_function handler,
  void *context)
{
  int local_index;
  struct sample_entry *entry;
495

496
  reader_alloc(d);
497
  local_index = labcomm_get_local_index(signature);
Anders Blomdell's avatar
Anders Blomdell committed
498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519
  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;
}