labcomm2006.c 7.79 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
51
52
int labcomm2006_reader_alloc(struct labcomm2006_reader *r, 
                         struct labcomm2006_reader_action_context *action_context, 
                         char *labcomm2006_version)
53
{
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
54
  UNWRAP(alloc, r, action_context, labcomm2006_version);
55
56
}

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

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

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

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

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
84
85
int labcomm2006_reader_ioctl(struct labcomm2006_reader *r, 
                         struct labcomm2006_reader_action_context *action_context,
86
                         int local_index, int remote_index,
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
87
                         struct labcomm2006_signature *signature, 
88
89
90
91
92
93
                         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
94
95
96
int labcomm2006_writer_alloc(struct labcomm2006_writer *w, 
                         struct labcomm2006_writer_action_context *action_context, 
                         char *labcomm2006_version)
97
{
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
98
  UNWRAP(alloc, w, action_context, labcomm2006_version);
99
100
}

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

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

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

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

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
127
128
int labcomm2006_writer_ioctl(struct labcomm2006_writer *w, 
                         struct labcomm2006_writer_action_context *action_context, 
129
                         int index, 
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
130
                         struct labcomm2006_signature *signature, 
131
132
133
134
135
136
137
138
139
140
141
                         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
142
static const char *labcomm2006_error_string[] = { 
143
#define LABCOMM_ERROR(name, description) description ,
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
144
#include "labcomm2006_error.h"
145
146
#undef LABCOMM_ERROR
};
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
147
148
static const int labcomm2006_error_string_count = (sizeof(labcomm2006_error_string) / 
					       sizeof(labcomm2006_error_string[0]));
149
150


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

Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
161
162
#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, ...)
163
164
{
#ifndef LABCOMM_NO_STDIO
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
165
  const char *err_msg = labcomm2006_error_get_str(error_id); // The final string to print.
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
  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
186
#endif
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205



#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
206
void *labcomm2006_signature_array_ref(struct labcomm2006_memory *memory,
207
208
209
210
211
212
				  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
213
    *data = labcomm2006_memory_alloc(memory, 0, size);
214
215
216
217
218
219
220
221
222
223
224
    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
225
    *data = labcomm2006_memory_alloc(memory, 0, n * size);
226
227
228
229
230
231
232
    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
233
    labcomm2006_memory_free(memory, 0, old_data);
234
235
236
237
238
239
240
241
242
243
244
  }
  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
245
void labcomm2006_set_local_index(struct labcomm2006_signature *signature)
246
247
{
  if (signature->index != 0) {
Sven Gestegård Robertz's avatar
Sven Gestegård Robertz committed
248
    labcomm2006_error_fatal_global(LABCOMM_ERROR_SIGNATURE_ALREADY_SET,
249
250
251
252
253
254
			       "%s", signature->name);
  }
  signature->index = local_index;
  local_index++;
}

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