labcomm2006_decoder.c 12.2 KB
Newer Older
1
/*
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
2
  labcomm2006_decoder.c -- runtime for handling decoding of labcomm samples.
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

  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/>.
*/
#include <errno.h>
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
22
23
24
25
#include "labcomm2006.h"
#include "labcomm2006_private.h"
#include "labcomm2006_ioctl.h"
#include "labcomm2006_dynamic_buffer_writer.h"
26
27
28

struct sample_entry {
  int remote_index;
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
29
30
31
  struct labcomm2006_signature *signature;
  labcomm2006_decoder_function decode;
  labcomm2006_handler_function handler;
32
33
34
  void *context;
};

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
35
36
struct labcomm2006_decoder {
  struct labcomm2006_reader *reader;
37
  int reader_allocated;
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
38
39
40
41
42
  struct labcomm2006_error_handler *error;
  struct labcomm2006_memory *memory;
  struct labcomm2006_scheduler *scheduler;
  labcomm2006_error_handler_callback on_error;
  labcomm2006_handle_new_datatype_callback on_new_datatype;
43
44
45
46
  LABCOMM_SIGNATURE_ARRAY_DEF(local, struct sample_entry);
  LABCOMM_SIGNATURE_ARRAY_DEF(remote_to_local, int);
};

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
47
48
49
50
51
struct labcomm2006_decoder *labcomm2006_decoder_new(
  struct labcomm2006_reader *reader,
  struct labcomm2006_error_handler *error,
  struct labcomm2006_memory *memory,
  struct labcomm2006_scheduler *scheduler)
52
{
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
53
  struct labcomm2006_decoder *result;
54

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
55
  result = labcomm2006_memory_alloc(memory, 0, sizeof(*result));
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
  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;
    result->reader_allocated = 0;
    result->error = error;
    result->memory = memory;
    result->scheduler = scheduler;
    LABCOMM_SIGNATURE_ARRAY_INIT(result->local, struct sample_entry);
    LABCOMM_SIGNATURE_ARRAY_INIT(result->remote_to_local, int);
  }
  return result;
}

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
74
void labcomm2006_decoder_free(struct labcomm2006_decoder* d)
75
{
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
76
  struct labcomm2006_memory *memory = d->memory;
77

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
78
  labcomm2006_reader_free(d->reader, d->reader->action_context);
79
80
  LABCOMM_SIGNATURE_ARRAY_FREE(memory, d->local, struct sample_entry);
  LABCOMM_SIGNATURE_ARRAY_FREE(memory, d->remote_to_local, int);
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
81
  labcomm2006_memory_free(memory, 0, d);
82
83
84
}

static int collect_flat_signature(
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
85
86
  struct labcomm2006_decoder *decoder,
  struct labcomm2006_writer *writer)
87
88
89
{
  int result, type;

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
90
  type = labcomm2006_read_packed32(decoder->reader); 
91
92
93
94
95
96
  result = decoder->reader->error;
  if (result < 0) { goto out; }
  if (type >= LABCOMM_USER) {
    decoder->on_error(LABCOMM_ERROR_UNIMPLEMENTED_FUNC, 3,
			"Implement %s ... (1) for type 0x%x\n", __FUNCTION__, type);
  } else {
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
97
    labcomm2006_write_packed32(writer, type); 
98
99
100
101
    switch (type) {
      case LABCOMM_ARRAY: {
	int dimensions, i;

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
102
103
	dimensions = labcomm2006_read_packed32(decoder->reader);
	labcomm2006_write_packed32(writer, dimensions);
104
	for (i = 0 ; i < dimensions ; i++) {
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
105
106
	  int n = labcomm2006_read_packed32(decoder->reader);
	  labcomm2006_write_packed32(writer, n);
107
108
109
110
111
112
113
	}
	result = collect_flat_signature(decoder, writer);
	if (result < 0) { goto out; }
      } break;
      case LABCOMM_STRUCT: {
	int fields, i;

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
114
115
	fields = labcomm2006_read_packed32(decoder->reader); 
	labcomm2006_write_packed32(writer, fields); 
116
	for (i = 0 ; i < fields ; i++) {
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
117
118
119
	  char *name = labcomm2006_read_string(decoder->reader);
	  labcomm2006_write_string(writer, name);
	  labcomm2006_memory_free(decoder->memory, 1, name);
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
	  result = collect_flat_signature(decoder, writer);
	  if (result < 0) { goto out; }
	}
      } 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: {
	result = -ENOSYS;
        decoder->on_error(LABCOMM_ERROR_UNIMPLEMENTED_FUNC, 3,
				"Implement %s (2) for type 0x%x...\n", __FUNCTION__, type);
      } break;
    }
  }
out:
  return result;
}

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
144
static int writer_ioctl(struct labcomm2006_writer *writer,
145
146
147
148
149
150
151
152
153
154
155
156
			uint32_t action,
			...)
{
  int result;
  va_list va;

  if (LABCOMM_IOC_SIG(action) != LABCOMM_IOC_NOSIG) {
    result = -EINVAL;
    goto out;
  }
  
  va_start(va, action);
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
157
  result = labcomm2006_writer_ioctl(writer, writer->action_context, 
158
159
160
161
162
163
				0, NULL, action, va);
  va_end(va);
out:
  return result;
}

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
164
static int decode_typedef_or_sample(struct labcomm2006_decoder *d, int kind)
165
166
167
{
  int result;

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
168
169
170
  /* TODO: should the labcomm2006_dynamic_buffer_writer be 
     a permanent part of labcomm2006_decoder? */
  struct labcomm2006_writer_action_context action_context = {
171
    .next = NULL,
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
172
    .action = labcomm2006_dynamic_buffer_writer_action,
173
174
    .context = NULL
  };
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
175
  struct labcomm2006_writer writer = {
176
177
178
179
180
181
182
183
    .action_context = &action_context,
    .memory = d->memory,
    .data = NULL,
    .data_size = 0,
    .count = 0,
    .pos = 0,
    .error = 0,
  };
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
184
  struct labcomm2006_signature signature, *local_signature;
185
186
187
188
  int remote_index, local_index, err;
  
  local_signature = NULL;
  local_index = 0;
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
189
190
191
192
  labcomm2006_writer_alloc(&writer, writer.action_context, "");
  labcomm2006_writer_start(&writer, writer.action_context, 0, NULL, NULL);
  remote_index = labcomm2006_read_packed32(d->reader);
  signature.name = labcomm2006_read_string(d->reader);
193
  collect_flat_signature(d, &writer);
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
194
  labcomm2006_writer_end(&writer, writer.action_context);
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
  err = writer_ioctl(&writer, 
		     LABCOMM_IOCTL_WRITER_GET_BYTES_WRITTEN,
		     &signature.size);
  if (err < 0) {
    fprintf(stderr, "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) {
    fprintf(stderr, "Failed to get pointer: %s\n", strerror(-err));
    result = -ENOENT;
    goto free_signature_name;
  }
  {
    int i;

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
214
    labcomm2006_scheduler_data_lock(d->scheduler);
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
    LABCOMM_SIGNATURE_ARRAY_FOREACH(d->local, struct sample_entry, i) {
      struct sample_entry *s;
      int *remote_to_local;
      
      result = -ENOENT;
      s = LABCOMM_SIGNATURE_ARRAY_REF(d->memory, 
				      d->local,  struct sample_entry, i);
      if (s->signature &&
	  s->signature->size == signature.size &&
	  strcmp(s->signature->name, signature.name) == 0 &&
	  memcmp((void*)s->signature->signature, (void*)signature.signature,
	       signature.size) == 0) {
	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;
      }
    }
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
238
    labcomm2006_scheduler_data_unlock(d->scheduler);
239
    if (local_signature) {
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
240
      labcomm2006_reader_start(d->reader, d->reader->action_context,
241
242
			   local_index, remote_index, local_signature,
			   NULL);
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
243
      labcomm2006_reader_end(d->reader, d->reader->action_context);
244
245
246
247
248
249
250
251
252
253
254
255
256
257
    }
  }
#if 0
  if (! entry) {
    /* 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;
#endif
free_signature_name:
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
258
259
  labcomm2006_memory_free(d->memory, 0, signature.name);
  labcomm2006_writer_free(&writer, writer.action_context);
260
261
262
263
  return result;
}

struct call_handler_context {
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
264
  struct labcomm2006_reader *reader;
265
266
  int local_index;
  int remote_index;
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
267
268
  struct labcomm2006_signature *signature;
  labcomm2006_handler_function handler;
269
270
271
272
273
274
275
276
  void *context;
};

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

  if (wrap->reader->error >= 0) {
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
277
    labcomm2006_reader_start(wrap->reader, wrap->reader->action_context,
278
279
280
			 wrap->local_index, wrap->remote_index, wrap->signature,
			 value);
    wrap->handler(value, wrap->context);
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
281
    labcomm2006_reader_end(wrap->reader, wrap->reader->action_context);
282
283
284
  }
}

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
285
static void reader_alloc(struct labcomm2006_decoder *d)
286
287
288
{
  if (!d->reader_allocated) {
    d->reader_allocated = 1;
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
289
    labcomm2006_reader_alloc(d->reader, d->reader->action_context,
290
291
292
293
			 LABCOMM_VERSION);
  }
}

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
294
int labcomm2006_decoder_decode_one(struct labcomm2006_decoder *d)
295
296
297
298
{
  int result, remote_index;

  reader_alloc(d);
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
299
  remote_index = labcomm2006_read_packed32(d->reader);
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
  if (d->reader->error < 0) {
    result = d->reader->error;
    goto out;
  }
  if (remote_index == LABCOMM_TYPEDEF || remote_index == LABCOMM_SAMPLE) {
    result = decode_typedef_or_sample(d, remote_index); 
  } else {
    int *local_index;
    struct call_handler_context wrap = {
      .reader = d->reader,
      .remote_index = remote_index,
      .signature = NULL,
      .handler = NULL,
      .context = NULL,
    };
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
315
    labcomm2006_decoder_function do_decode = NULL;
316

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
317
    labcomm2006_scheduler_data_lock(d->scheduler);
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
    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;
    }
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
334
    labcomm2006_scheduler_data_unlock(d->scheduler);
335
336
337
338
339
340
341
342
343
344
345
346
347
    if (do_decode) {
      do_decode(d->reader, call_handler, &wrap);
      if (d->reader->error < 0) {
	result = d->reader->error;
      }
    } else {
      result = -ENOENT;
    }
  }
out:
  return result;
}

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
348
void labcomm2006_decoder_run(struct labcomm2006_decoder *d)
349
{
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
350
  while (labcomm2006_decoder_decode_one(d) > 0) {
351
352
353
  }
}

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
354
int labcomm2006_decoder_ioctl(struct labcomm2006_decoder *d, 
355
356
357
358
359
360
361
			  uint32_t action,
			  ...)
{
  int result;  
  va_list va;
    
  va_start(va, action);
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
362
  result = labcomm2006_reader_ioctl(d->reader, 
363
364
365
366
367
368
				d->reader->action_context,
				0, 0, NULL, action, va);
  va_end(va);
  return result;
}

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
369
370
int labcomm2006_internal_decoder_ioctl(struct labcomm2006_decoder *d, 
				   struct labcomm2006_signature *signature,
371
372
373
374
375
				   uint32_t action, va_list va)
{
  int result;
  int local_index, remote_index;

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
376
377
  local_index = labcomm2006_get_local_index(signature);
  labcomm2006_scheduler_data_lock(d->scheduler);
378
379
380
381
  remote_index = LABCOMM_SIGNATURE_ARRAY_REF(d->memory,
					     d->local,
					     struct sample_entry,
					     local_index)->remote_index;
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
382
383
  labcomm2006_scheduler_data_unlock(d->scheduler);
  result = labcomm2006_reader_ioctl(d->reader, d->reader->action_context,
384
385
386
387
388
				local_index, remote_index, 
				signature, action, va);
  return result;
}

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
389
390
391
392
393
int labcomm2006_internal_decoder_register(
  struct labcomm2006_decoder *d,
  struct labcomm2006_signature *signature,
  labcomm2006_decoder_function decode, 
  labcomm2006_handler_function handler,
394
395
396
397
398
399
  void *context)
{
  int local_index;
  struct sample_entry *entry;
 
  reader_alloc(d);
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
400
  local_index = labcomm2006_get_local_index(signature);
401
  if (local_index <= 0) { goto out; }
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
402
  labcomm2006_reader_start(d->reader, d->reader->action_context,
403
404
		       local_index, 0, signature,
		       NULL);
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
405
  labcomm2006_reader_end(d->reader, d->reader->action_context);
406

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
407
  labcomm2006_scheduler_data_lock(d->scheduler);
408
409
410
411
412
413
414
415
416
417
  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:
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
418
  labcomm2006_scheduler_data_unlock(d->scheduler);
419
420
421
422
out:
  return local_index;
}