labcomm2006.c 7.87 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
/*
  labcomm.c -- runtime for handling encoding and 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/>.
*/

#ifdef LABCOMM_COMPAT
  #include LABCOMM_COMPAT
#else
  #include <stdio.h>
  #include <strings.h>
#endif

#include <errno.h>
#include <string.h>
#include <stdarg.h>
#include <stddef.h>

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
35
36
37
38
#include "labcomm2006.h"
#include "labcomm2006_private.h"
#include "labcomm2006_ioctl.h"
#include "labcomm2006_dynamic_buffer_writer.h"
39
40
41
42
43
44
45
46
47
48
49

/* Unwrapping reader/writer functions */
#define UNWRAP_ac(rw, ac, ...) ac
#define UNWRAP(func, ...)	     \
  while (1) {								\
    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;		\
  }

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
50
int labcomm2006_reader_alloc(struct labcomm2006_reader *r, 
51
                         struct labcomm2006_reader_action_context *action_context)
52
{
53
  UNWRAP(alloc, r, action_context);
54
55
}

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
56
57
int labcomm2006_reader_free(struct labcomm2006_reader *r, 
                        struct labcomm2006_reader_action_context *action_context)
58
59
60
61
{
  UNWRAP(free, r, action_context);
}

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
62
63
int labcomm2006_reader_start(struct labcomm2006_reader *r, 
                         struct labcomm2006_reader_action_context *action_context,
64
			 int local_index, int remote_index,
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
65
			 struct labcomm2006_signature *signature,
66
67
68
69
70
			 void *value)
{
  UNWRAP(start, r, action_context, local_index, remote_index, signature, value);
}

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
71
72
int labcomm2006_reader_end(struct labcomm2006_reader *r, 
                       struct labcomm2006_reader_action_context *action_context)
73
74
75
76
{
  UNWRAP(end, r, action_context);
}

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
77
78
int labcomm2006_reader_fill(struct labcomm2006_reader *r, 
                        struct labcomm2006_reader_action_context *action_context)
79
80
81
82
{
  UNWRAP(fill, r, action_context);
}

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
83
84
int labcomm2006_reader_ioctl(struct labcomm2006_reader *r, 
                         struct labcomm2006_reader_action_context *action_context,
85
                         int local_index, int remote_index,
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
86
                         struct labcomm2006_signature *signature, 
87
88
89
90
91
92
                         uint32_t ioctl_action, va_list args)
{
  UNWRAP(ioctl, r, action_context, 
	 local_index, remote_index, signature, ioctl_action, args);
}

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
93
int labcomm2006_writer_alloc(struct labcomm2006_writer *w, 
94
                         struct labcomm2006_writer_action_context *action_context)
95
{
96
  UNWRAP(alloc, w, action_context);
97
98
}

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
99
100
int labcomm2006_writer_free(struct labcomm2006_writer *w, 
                        struct labcomm2006_writer_action_context *action_context)
101
102
103
104
{
  UNWRAP(free, w, action_context);
}

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
105
106
107
int labcomm2006_writer_start(struct labcomm2006_writer *w, 
                         struct labcomm2006_writer_action_context *action_context,
                         int index, struct labcomm2006_signature *signature,
108
109
110
111
112
                         void *value)
{
  UNWRAP(start, w, action_context, index, signature, value);
}

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
113
114
int labcomm2006_writer_end(struct labcomm2006_writer *w, 
                       struct labcomm2006_writer_action_context *action_context)
115
116
117
118
{
  UNWRAP(end, w, action_context);
} 

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
119
120
int labcomm2006_writer_flush(struct labcomm2006_writer *w, 
                         struct labcomm2006_writer_action_context *action_context)
121
122
123
124
{
  UNWRAP(flush, w, action_context);
} 

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
125
126
int labcomm2006_writer_ioctl(struct labcomm2006_writer *w, 
                         struct labcomm2006_writer_action_context *action_context, 
127
                         int index, 
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
128
                         struct labcomm2006_signature *signature, 
129
130
131
132
133
134
135
136
137
138
139
                         uint32_t ioctl_action, va_list args)
{
  UNWRAP(ioctl, w, action_context, index, signature, ioctl_action, args);
} 

#undef UNWRAP
#undef UNWRAP_ac




Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
140
static const char *labcomm2006_error_string[] = { 
141
#define LABCOMM_ERROR(name, description) description ,
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
142
#include "labcomm2006_error.h"
143
144
#undef LABCOMM_ERROR
};
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
145
146
static const int labcomm2006_error_string_count = (sizeof(labcomm2006_error_string) / 
					       sizeof(labcomm2006_error_string[0]));
147
148


Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
149
const char *labcomm2006_error_get_str(enum labcomm2006_error error_id)
150
151
152
{
  const char *error_str = NULL;
  // Check if this is a known error ID.
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
153
154
  if (error_id < labcomm2006_error_string_count) {
    error_str = labcomm2006_error_string[error_id];
155
156
157
158
  }
  return error_str;
}

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
159
160
#if 0 //XXX hack to avoid name clash. is this really the same as in 2013?
void on_error_fprintf(enum labcomm2006_error error_id, size_t nbr_va_args, ...)
161
162
{
#ifndef LABCOMM_NO_STDIO
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
163
  const char *err_msg = labcomm2006_error_get_str(error_id); // The final string to print.
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
  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
}
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
184
#endif
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203



#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

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
204
void *labcomm2006_signature_array_ref(struct labcomm2006_memory *memory,
205
206
207
208
209
210
				  int *first, int *last, void **data,
				  int size, int index)
{
  if (*first == 0 && *last == 0) {
    *first = index;
    *last = index + 1;
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
211
    *data = labcomm2006_memory_alloc(memory, 0, size);
212
213
214
215
216
217
218
219
220
221
222
    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);
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
223
    *data = labcomm2006_memory_alloc(memory, 0, n * size);
224
225
226
227
228
229
230
    if (*data) {
      memset(*data, 0, n * size);
      memcpy(*data + (old_first - *first) * size, 
	     old_data, 
	     (old_last - old_first) * size);
    }
//    dump(old_data, size, old_first, old_last);
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
231
    labcomm2006_memory_free(memory, 0, old_data);
232
233
234
235
236
237
238
239
240
241
242
  }
  if (*data) {
//    dump(*data, size, *first, *last);
    return *data + (index - *first) * size;
  } else {
    return NULL;
  }
}

static int local_index = 0x40;

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
243
void labcomm2006_set_local_index(struct labcomm2006_signature *signature)
244
245
{
  if (signature->index != 0) {
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
246
    labcomm2006_error_fatal_global(LABCOMM_ERROR_SIGNATURE_ALREADY_SET,
247
248
249
250
251
252
			       "%s", signature->name);
  }
  signature->index = local_index;
  local_index++;
}

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
253
int labcomm2006_get_local_index(struct labcomm2006_signature *signature)
254
255
{
  if (signature->index == 0) {
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
256
    labcomm2006_error_fatal_global(LABCOMM_ERROR_SIGNATURE_NOT_SET,
257
258
259
260
			       "%s", signature->name);
  }
  return signature->index;
}
Anders Blomdell's avatar
Anders Blomdell committed
261
262
263
264
265
266
267
268

int labcomm2006_internal_sizeof(struct labcomm2006_signature *signature,
                            void *v)
{
  int length = signature->encoded_size(v);
  return (labcomm2006_size_packed32(signature->index) +
          length);
}