po_hi_driver_rasta_1553_brmlib.c 11.8 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
/*
 * This is a part of PolyORB-HI-C distribution, a minimal
 * middleware written for generated code from AADL models.
 * You should use it with the Ocarina toolsuite.
 *
 * Copyright (C) 2010, European Space Agency
 */

/*
 * This code has been greatly inspired by GAISLER examples.
 */

#include <deployment.h>
/* Generated code header */

#ifdef __PO_HI_NEED_DRIVER_1553_RASTA

#include <rtems.h>
#include <stdlib.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

#include <b1553brm.h>
julien.delange's avatar
julien.delange committed
26
#include <po_hi_debug.h>
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
#include <drivers/po_hi_driver_rasta_1553_brmlib.h>

/* The stupid rtems name to errno table, in fact I hate it.... :)
 *
rtems_assoc_t errno_assoc[] = {
    { "OK",                 RTEMS_SUCCESSFUL,                0 },
    { "BUSY",               RTEMS_RESOURCE_IN_USE,           EBUSY },
    { "INVALID NAME",       RTEMS_INVALID_NAME,              EINVAL },
    { "NOT IMPLEMENTED",    RTEMS_NOT_IMPLEMENTED,           ENOSYS },
    { "TIMEOUT",            RTEMS_TIMEOUT,                   ETIMEDOUT },
    { "NO MEMORY",          RTEMS_NO_MEMORY,                 ENOMEM },
    { "NO DEVICE",          RTEMS_UNSATISFIED,               ENODEV },
    { "INVALID NUMBER",     RTEMS_INVALID_NUMBER,            EBADF},
    { "NOT RESOURCE OWNER", RTEMS_NOT_OWNER_OF_RESOURCE,     EPERM},
    { "IO ERROR",           RTEMS_IO_ERROR,                  EIO},
    { 0, 0, 0 },
};
*/

__po_hi_c_driver_rasta_1553_brm_t __po_hi_c_driver_1553_rasta_brmlib_open(char *driver_name)
{
	int fd;
	__po_hi_c_driver_rasta_1553_brm_t ret = NULL;
	
julien.delange's avatar
julien.delange committed
51
	__DEBUGMSG("[RASTA 1553 BRMLIB] Opening driver %s ...",driver_name);
52
53
	
	fd = open(driver_name,O_RDWR);
julien.delange's avatar
julien.delange committed
54
55
	if ( fd >= 0 )
   {
56
57
58
59
		ret = calloc(sizeof(*ret),1);
		ret->fd = fd;
		/* Initial state of driver */
		ret->mode = BRM_MODE_RT;
julien.delange's avatar
julien.delange committed
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
	   __DEBUGMSG("OK !\n");
	
	}
   else
   {
		if ( errno == ENODEV )
      {
			__DEBUGMSG(" driver %s doesn't exist\n",driver_name);
		}
      else
      {
         if ( errno == EBUSY )
         {
			   __DEBUGMSG(" channel already taken\n");
		   }
         else
         {
			   __DEBUGMSG(" unknown error, errno: %d, ret: %d\n",errno,fd);
         }
79
80
81
82
83
84
85
86
		}
	}
	
	return ret;
}

void __po_hi_c_driver_1553_rasta_brmlib_close(__po_hi_c_driver_rasta_1553_brm_t chan){
	if ( !chan || (chan->fd<0) )
julien.delange's avatar
julien.delange committed
87
   {
88
		return;
julien.delange's avatar
julien.delange committed
89
   }
90
91
92
93
	close(chan->fd);
	free(chan);
}

julien.delange's avatar
julien.delange committed
94
95
96
int __po_hi_c_driver_1553_rasta_brmlib_rt_send_multiple(__po_hi_c_driver_rasta_1553_brm_t chan, struct rt_msg *msgs, int msgcnt)
{

97
	int ret;
julien.delange's avatar
julien.delange committed
98
99
100

	if ( !chan || !msgs || (msgcnt < 0) )
   {
101
		return -1;
julien.delange's avatar
julien.delange committed
102
   }
103
104
	
	if ( msgcnt == 0 )
julien.delange's avatar
julien.delange committed
105
   {
106
		return 0;
julien.delange's avatar
julien.delange committed
107
   }
108
109
	
	ret = write(chan->fd,msgs,msgcnt);
julien.delange's avatar
julien.delange committed
110
111
112

	if ( ret < 0 )
   {
113
114
115
116
		/* something went wrong 
		 * OR in non-blocking mode
		 * that would block
		 */
julien.delange's avatar
julien.delange committed
117
118
		if ( !chan->txblk && (errno == EBUSY) )
      {
119
120
121
122
			/* would block ==> 0 sent is ok */
			return 0;
		}
			
julien.delange's avatar
julien.delange committed
123
124
		if ( errno == EINVAL )
      {
125
			/* CAN must be started before receiving */
julien.delange's avatar
julien.delange committed
126
			__DEBUGMSG("[RASTA 1553 BRMLIB] input descriptor numbering error\n");
127
128
129
			return -1;
		}

julien.delange's avatar
julien.delange committed
130
		__DEBUGMSG("[RASTA 1553 BRMLIB] error in write, errno: %d, returned: %d\n",errno,ret);
131
132
133
134
135
136
137
		return -1;
	}
	
	/* sent all of them */
	return ret;
}

julien.delange's avatar
julien.delange committed
138
139
140
int __po_hi_c_driver_1553_rasta_brmlib_rt_send(__po_hi_c_driver_rasta_1553_brm_t chan, struct rt_msg *msg)
{
   return __po_hi_c_driver_1553_rasta_brmlib_rt_send_multiple(chan,msg,1);
141
142
}

julien.delange's avatar
julien.delange committed
143
144
int __po_hi_c_driver_1553_rasta_brmlib_recv_multiple(__po_hi_c_driver_rasta_1553_brm_t chan, void *msgs, int msglen)
{
145
146
147
	int ret;
	
	if ( !chan || !msgs || (msglen<0) )
julien.delange's avatar
julien.delange committed
148
   {
149
		return -1;
julien.delange's avatar
julien.delange committed
150
   }
151
152
	
	if ( msglen == 0 )
julien.delange's avatar
julien.delange committed
153
   {
154
		return 0;
julien.delange's avatar
julien.delange committed
155
   }
156
157
	
	errno = 0;
julien.delange's avatar
julien.delange committed
158

159
	ret = read(chan->fd,msgs,msglen);
julien.delange's avatar
julien.delange committed
160
161
162

	if ( ret < 0 )
   {
163
164
165
166
		/* something went wrong 
		 * OR in non-blocking mode
		 * that would block
		 */
julien.delange's avatar
julien.delange committed
167
168
		if ( !chan->rxblk && (errno == EBUSY) )
      {
169
170
171
			return 0;
		}
		
julien.delange's avatar
julien.delange committed
172
		__DEBUGMSG("[RASTA 1553 BRMLIB] error in read, errno: %d, returned: %d\n",errno,ret);
173
174
175
176
177
178
179
180
181
182
		return -1;
	}
	
	/* message count is returned, not byte count */
	return ret;
}

int __po_hi_c_driver_1553_rasta_brmlib_rt_recv_multiple(__po_hi_c_driver_rasta_1553_brm_t chan, struct rt_msg *msgs, int msgcnt)
{
	if ( !chan || (chan->mode!=BRM_MODE_RT) )
julien.delange's avatar
julien.delange committed
183
   {
184
		return -1;
julien.delange's avatar
julien.delange committed
185
   }
186
187
188
189
190
191
192
193
	
	/* Read the messages */
	return __po_hi_c_driver_1553_rasta_brmlib_recv_multiple(chan,(void *)msgs,msgcnt);
}

int __po_hi_c_driver_1553_rasta_brmlib_bm_recv_multiple(__po_hi_c_driver_rasta_1553_brm_t chan, struct bm_msg *msgs, int msgcnt)
{
	if ( !chan || (chan->mode!=BRM_MODE_BM) )
julien.delange's avatar
julien.delange committed
194
   {
195
		return -1;
julien.delange's avatar
julien.delange committed
196
   }
197
198
199
200
201
	
	/* Read the messages */
	return __po_hi_c_driver_1553_rasta_brmlib_recv_multiple(chan,(void *)msgs,msgcnt);
}

julien.delange's avatar
julien.delange committed
202
203
int __po_hi_c_driver_1553_rasta_brmlib_rt_recv(__po_hi_c_driver_rasta_1553_brm_t chan, struct rt_msg *msg)
{
204
205
206
	return __po_hi_c_driver_1553_rasta_brmlib_rt_recv_multiple(chan,msg,1);
}

julien.delange's avatar
julien.delange committed
207
208
209

int __po_hi_c_driver_1553_rasta_brmlib_bm_recv(__po_hi_c_driver_rasta_1553_brm_t chan, struct bm_msg *msg)
{
210
211
212
213
	return __po_hi_c_driver_1553_rasta_brmlib_bm_recv_multiple(chan,msg,1);
}


julien.delange's avatar
julien.delange committed
214
215
int __po_hi_c_driver_1553_rasta_brmlib_set_mode(__po_hi_c_driver_rasta_1553_brm_t chan, unsigned int mode)
{
216
217
218
219
	int ret;
	unsigned int arg = mode;
	
	if ( !chan )
julien.delange's avatar
julien.delange committed
220
   {
221
		return -1;
julien.delange's avatar
julien.delange committed
222
   }
223
224
	
	ret = ioctl(chan->fd,BRM_SET_MODE,&arg);
julien.delange's avatar
julien.delange committed
225
226
227
228
229
230

	if ( ret < 0 )
   {
		if ( errno == EINVAL )
      {
			__DEBUGMSG("[RASTA 1553 BRMLIB] set_mode invalid mode: %d\n",arg);
231
232
233
			return -2;
		}
	
julien.delange's avatar
julien.delange committed
234
235
		if ( errno == ENOMEM )
      {
236
			/* started */
julien.delange's avatar
julien.delange committed
237
			__DEBUGMSG("[RASTA 1553 BRMLIB] set_mode: not enough memory\n");
238
239
240
241
			return -3;
		}
		
		/* unhandeled errors */
julien.delange's avatar
julien.delange committed
242
		__DEBUGMSG("[RASTA 1553 BRMLIB] set_mode: failed, errno: %d, ret: %d\n",errno,ret);
243
244
245
246
247
248
249
250
251
		return -1;
	}
	
	/* Save mode */
	chan->mode = mode;
	
	return 0;
}

julien.delange's avatar
julien.delange committed
252
253
int __po_hi_c_driver_1553_rasta_brmlib_set_bus(__po_hi_c_driver_rasta_1553_brm_t chan, unsigned int bus)
{
254
255
256
	int ret;
	unsigned int arg = bus;
	if ( !chan )
julien.delange's avatar
julien.delange committed
257
   {
258
		return -1;
julien.delange's avatar
julien.delange committed
259
   }
260
261
		
	/* only for RT mode */
julien.delange's avatar
julien.delange committed
262
263
264
	if ( chan->mode != BRM_MODE_RT )
   {
		__DEBUGMSG("[RASTA 1553 BRMLIB] set_bus: Only possible to set bus in RT mode\n");
265
266
267
268
		return -2; /* fast EINVAL... */
	}
	
	ret = ioctl(chan->fd,BRM_SET_BUS,&arg);
julien.delange's avatar
julien.delange committed
269
270
271
272
273
274

	if ( ret < 0 )
   {
		if ( errno == EINVAL )
      {
			__DEBUGMSG("[RASTA 1553 BRMLIB] set_bus: invalid bus: %d\n",arg);
275
276
277
278
			return -2;
		}
		
		/* unhandeled errors */
julien.delange's avatar
julien.delange committed
279
		__DEBUGMSG("[RASTA 1553 BRMLIB] set_bus: failed, errno: %d, ret: %d\n",errno,ret);
280
281
282
283
284
		return -1;
	}
	return 0;
}

julien.delange's avatar
julien.delange committed
285
286
int __po_hi_c_driver_1553_rasta_brmlib_set_msg_timeout(__po_hi_c_driver_rasta_1553_brm_t chan, unsigned int timeout)
{
287
288
289
290
	int ret;
	unsigned int arg = timeout;
	
	if ( !chan )
julien.delange's avatar
julien.delange committed
291
   {
292
		return -1;
julien.delange's avatar
julien.delange committed
293
   }
294
	
julien.delange's avatar
julien.delange committed
295
296
297
	if ( !((chan->mode==BRM_MODE_BM) || (chan->mode==BRM_MODE_BC)) )
   {
		__DEBUGMSG("[RASTA 1553 BRMLIB] set_msg_timeout: Only possible to set bus in BC & BM mode\n");
298
299
300
301
		return -2;
	}
	
	ret = ioctl(chan->fd,BRM_SET_MSGTO,&arg);
julien.delange's avatar
julien.delange committed
302
303
304
305
306

	if ( ret < 0 )
   {
		if ( errno == EBUSY )
      {
307
			/* started */
julien.delange's avatar
julien.delange committed
308
			__DEBUGMSG("[RASTA 1553 BRMLIB] set_msg_timeout: started\n");
309
310
311
			return -2;
		}
		
julien.delange's avatar
julien.delange committed
312
		__DEBUGMSG("[RASTA 1553 BRMLIB] set_msg_timeout: failed, errno: %d, ret: %d\n",errno,ret);
313
314
315
316
317
318
		return -1;
	}
	
	return 0;
}

julien.delange's avatar
julien.delange committed
319
320
int __po_hi_c_driver_1553_rasta_brmlib_set_rt_addr(__po_hi_c_driver_rasta_1553_brm_t chan, unsigned int address)
{
321
322
323
324
	int ret;
	unsigned int arg = address;
	
	if ( !chan )
julien.delange's avatar
julien.delange committed
325
   {
326
		return -1;
julien.delange's avatar
julien.delange committed
327
   }
328
	
julien.delange's avatar
julien.delange committed
329
330
331
	if ( chan->mode != BRM_MODE_RT )
   {
		__DEBUGMSG("[RASTA 1553 BRMLIB] set_rt_addr: not in RT mode\n");
332
333
334
335
		return -2;
	}
	
	ret = ioctl(chan->fd,BRM_SET_RT_ADDR,&arg);
julien.delange's avatar
julien.delange committed
336
337
338
339

	if ( ret < 0 )
   {
		__DEBUGMSG("[RASTA 1553 BRMLIB] set_rt_addr: failed, errno: %d, ret: %d\n",errno,ret);
340
341
342
343
344
345
346
347
348
349
350
		return -1;
	}
	
	return 0;
}

int __po_hi_c_driver_1553_rasta_brmlib_set_std(__po_hi_c_driver_rasta_1553_brm_t chan, int std){
	int ret;
	unsigned int arg = std;
	
	if ( !chan )
julien.delange's avatar
julien.delange committed
351
   {
352
		return -1;
julien.delange's avatar
julien.delange committed
353
   }
354
355
	
	ret = ioctl(chan->fd,BRM_SET_STD,&arg);
julien.delange's avatar
julien.delange committed
356
357
358
359
360

	if ( ret < 0 )
   {
		if ( errno == EINVAL )
      {
361
			/* started */
julien.delange's avatar
julien.delange committed
362
			__DEBUGMSG("[RASTA 1553 BRMLIB] set_std: new standard not valid: %d\n",arg);
363
364
365
			return -2;
		}
		
julien.delange's avatar
julien.delange committed
366
		__DEBUGMSG("[RASTA 1553 BRMLIB] set_filter: failed, errno: %d, ret: %d\n",errno,ret);
367
368
369
370
371
372
373
		return -1;
	}
	
	return 0;
}


julien.delange's avatar
julien.delange committed
374
375
int __po_hi_c_driver_1553_rasta_brmlib_set_txblock(__po_hi_c_driver_rasta_1553_brm_t chan, int txblocking)
{
376
377
378
379
	unsigned int arg = (txblocking) ? 1 : 0;
	int ret;
	
	if ( !chan )
julien.delange's avatar
julien.delange committed
380
   {
381
		return -1;
julien.delange's avatar
julien.delange committed
382
   }
383
384
	
	ret = ioctl(chan->fd,BRM_TX_BLOCK,&arg);
julien.delange's avatar
julien.delange committed
385
386
387
388

	if ( ret < 0 )
   {
		__DEBUGMSG("[RASTA 1553 BRMLIB] set_txblock: failed, errno: %d, ret: %d\n",errno,ret);
389
390
		return -1;
	}
julien.delange's avatar
julien.delange committed
391

392
393
394
395
396
397
398
399
400
401
402
	/* remember blocking state */
	chan->txblk = arg;
	return 0;
}


int __po_hi_c_driver_1553_rasta_brmlib_set_rxblock(__po_hi_c_driver_rasta_1553_brm_t chan, int rxblocking){
	unsigned int arg = (rxblocking) ? 1 : 0;
	int ret;
	
	if ( !chan )
julien.delange's avatar
julien.delange committed
403
   {
404
		return -1;
julien.delange's avatar
julien.delange committed
405
   }
406
407
	
	ret = ioctl(chan->fd,BRM_RX_BLOCK,&arg);
julien.delange's avatar
julien.delange committed
408
409
410
411

	if ( ret < 0 )
   {
		__DEBUGMSG("[RASTA 1553 BRMLIB] set_rxblock: failed, errno: %d, ret: %d\n",errno,ret);
412
413
		return -1;
	}
julien.delange's avatar
julien.delange committed
414

415
416
	/* remember blocking state */
	chan->rxblk = arg;
julien.delange's avatar
julien.delange committed
417

418
419
420
421
422
	return 0;
}

int __po_hi_c_driver_1553_rasta_brmlib_set_block(__po_hi_c_driver_rasta_1553_brm_t chan, int txblocking, int rxblocking){
	int ret;
julien.delange's avatar
julien.delange committed
423

424
	ret = __po_hi_c_driver_1553_rasta_brmlib_set_txblock(chan,txblocking);
julien.delange's avatar
julien.delange committed
425
426
427

	if ( !ret )
   {
428
429
		return __po_hi_c_driver_1553_rasta_brmlib_set_rxblock(chan,rxblocking);
	}
julien.delange's avatar
julien.delange committed
430

431
432
433
434
435
436
437
438
	return ret;
}

int __po_hi_c_driver_1553_rasta_brmlib_set_broadcast(__po_hi_c_driver_rasta_1553_brm_t chan, int broadcast){
	unsigned int arg = (broadcast) ? 1 : 0;
	int ret;
	
	if ( !chan )
julien.delange's avatar
julien.delange committed
439
   {
440
		return -1;
julien.delange's avatar
julien.delange committed
441
   }
442
443
	
	ret = ioctl(chan->fd,BRM_SET_BCE,&arg);
julien.delange's avatar
julien.delange committed
444
445
446
447

	if ( ret < 0 )
   {
		__DEBUGMSG("[RASTA 1553 BRMLIB] set_broadcast: failed, errno: %d, ret: %d\n",errno,ret);
448
449
450
451
452
453
454
455
		return -1;
	}
	
	/* remember broadcast state */
	chan->broadcast = arg;
	return 0;
}

julien.delange's avatar
julien.delange committed
456
457
int __po_hi_c_driver_1553_rasta_brmlib_bc_dolist(__po_hi_c_driver_rasta_1553_brm_t chan, struct bc_msg *msgs)
{
458
459
460
	int ret;
	
	ret = ioctl(chan->fd, BRM_DO_LIST, msgs);
julien.delange's avatar
julien.delange committed
461
462
463
464
465
466

	if ( ret < 0 )
   {
		if ( errno == EINVAL )
      {
			__DEBUGMSG("[RASTA 1553 BRMLIB] bc_dolist: not in BC mode\n");
467
468
469
			return -2;
		}

julien.delange's avatar
julien.delange committed
470
471
472
		if ( errno == EBUSY )
      {
			__DEBUGMSG("[RASTA 1553 BRMLIB] bc_dolist: busy\n");
473
474
475
			return -3;
		}
		
julien.delange's avatar
julien.delange committed
476
		__DEBUGMSG("[RASTA 1553 BRMLIB] dolist: errno %d, ret: %d\n",errno,ret);
477
478
479
480
481
		return -1;
	}
	return 0;
}

julien.delange's avatar
julien.delange committed
482
483
int __po_hi_c_driver_1553_rasta_brmlib_bc_dolist_wait(__po_hi_c_driver_rasta_1553_brm_t chan)
{
484
485
486
487
	int ret;
	unsigned int result;
	
	ret = ioctl(chan->fd, BRM_LIST_DONE, &result);
julien.delange's avatar
julien.delange committed
488
489
490
491
492
493

	if ( ret < 0 )
   {
		if ( errno == EINVAL )
      {
			__DEBUGMSG("[RASTA 1553 BRMLIB] bc_dolist: not in BC mode\n");
494
495
496
			return -2;
		}

julien.delange's avatar
julien.delange committed
497
498
499
		if ( errno == EBUSY )
      {
			__DEBUGMSG("[RASTA 1553 BRMLIB] dolist: busy\n");
500
501
502
			return -3;
		}
		
julien.delange's avatar
julien.delange committed
503
		__DEBUGMSG("[RASTA 1553 BRMLIB] bc_dolist: errno %d, ret: %d\n",errno,ret);
504
505
506
507
508
509
		return -1;
	}
	
	return result;
}

julien.delange's avatar
julien.delange committed
510
511
void __po_hi_c_driver_rasta_1553_print_rt_msg(int i, struct rt_msg *msg)
{
512
	int k, wc;
julien.delange's avatar
julien.delange committed
513

514
	wc = msg->miw >> 11;
julien.delange's avatar
julien.delange committed
515
516
517

	__DEBUGMSG("[RASTA 1553 BRMLIB] MSG[%d]: miw: 0x%x, time: 0x%x, desc: 0x%x, len: %d\n  ",i,msg->miw,msg->time,msg->desc,wc);

518
	/* print data */			
julien.delange's avatar
julien.delange committed
519
520
521
522
523
524
525
526
527
	for (k = 0; k < wc; k++)
   {
		if ( isalnum(msg->data[k]) )
      {
         __DEBUGMSG("0x%x (%c)",msg->data[k],msg->data[k]);
		}
      else
      {
			__DEBUGMSG("0x%x (.)",msg->data[k]);
528
529
530
		}
	}
	if ( k > 0 )
julien.delange's avatar
julien.delange committed
531
532
533
   {
		__DEBUGMSG("\n");
   }
534
535
}

julien.delange's avatar
julien.delange committed
536
537
void __po_hi_c_driver_rasta_1553_print_bm_msg(int i, struct bm_msg *msg)
{
538
	int k,wc,tr,desc;
julien.delange's avatar
julien.delange committed
539

540
	wc = msg->cw1 & 0x1f;	
julien.delange's avatar
julien.delange committed
541

542
	desc = (msg->cw1 >> 5) & 0x1f;
julien.delange's avatar
julien.delange committed
543

544
545
	tr = msg->cw1 & 0x0400;

julien.delange's avatar
julien.delange committed
546
547
548
	__DEBUGMSG("[RASTA 1553 BRMLIB] MSG[%d]: miw: 0x%x, cw1: 0x%x, cw2: 0x%x, desc: %d\n",i,msg->miw,msg->cw1,msg->cw2,desc);

	__DEBUGMSG("         sw1: 0x%x, sw2: 0x%x, tim: 0x%x, len: %d\n",msg->sw1,msg->sw2,msg->time,wc);
549
550
551
	
	/* no message data in BC transmit commands */
	if ( tr )
julien.delange's avatar
julien.delange committed
552
   {
553
		return;
julien.delange's avatar
julien.delange committed
554
   }
555
	
julien.delange's avatar
julien.delange committed
556
	__DEBUGMSG("         ");
557
558
	
	/* print data */			
julien.delange's avatar
julien.delange committed
559
560
561
562
563
564
565
566
567
	for (k = 0; k<wc; k++)
   {
		if ( isalnum(msg->data[k]) )
      {
         __DEBUGMSG("0x%x (%c)",msg->data[k],msg->data[k]);
		}
      else
      {
			__DEBUGMSG("0x%x (.)",msg->data[k]);
568
569
		}
	}
julien.delange's avatar
julien.delange committed
570

571
	if ( k > 0 )
julien.delange's avatar
julien.delange committed
572
573
574
   {
		__DEBUGMSG("\n");
   }
575
576
577
578
}

#endif