po_hi_driver_rasta_1553_brmlib.c 11.9 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
		__DEBUGMSG("[RASTA 1553 BRMLIB] zero length, errno: %d, returned: %d\n",errno,ret);
155
		return 0;
julien.delange's avatar
julien.delange committed
156
   }
157
158
	
	errno = 0;
julien.delange's avatar
julien.delange committed
159

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

	if ( ret < 0 )
   {
164
165
166
167
		/* something went wrong 
		 * OR in non-blocking mode
		 * that would block
		 */
julien.delange's avatar
julien.delange committed
168
169
		if ( !chan->rxblk && (errno == EBUSY) )
      {
170
171
172
			return 0;
		}
		
julien.delange's avatar
julien.delange committed
173
		__DEBUGMSG("[RASTA 1553 BRMLIB] error in read, errno: %d, returned: %d\n",errno,ret);
174
175
176
177
178
179
180
181
182
183
		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
184
   {
185
		return -1;
julien.delange's avatar
julien.delange committed
186
   }
187
188
189
190
191
192
193
194
	
	/* 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
195
   {
196
		return -1;
julien.delange's avatar
julien.delange committed
197
   }
198
199
200
201
202
	
	/* Read the messages */
	return __po_hi_c_driver_1553_rasta_brmlib_recv_multiple(chan,(void *)msgs,msgcnt);
}

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

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

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


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

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

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

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

julien.delange's avatar
julien.delange committed
286
287
int __po_hi_c_driver_1553_rasta_brmlib_set_msg_timeout(__po_hi_c_driver_rasta_1553_brm_t chan, unsigned int timeout)
{
288
289
290
291
	int ret;
	unsigned int arg = timeout;
	
	if ( !chan )
julien.delange's avatar
julien.delange committed
292
   {
293
		return -1;
julien.delange's avatar
julien.delange committed
294
   }
295
	
julien.delange's avatar
julien.delange committed
296
297
298
	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");
299
300
301
302
		return -2;
	}
	
	ret = ioctl(chan->fd,BRM_SET_MSGTO,&arg);
julien.delange's avatar
julien.delange committed
303
304
305
306
307

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

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

	if ( ret < 0 )
   {
		__DEBUGMSG("[RASTA 1553 BRMLIB] set_rt_addr: failed, errno: %d, ret: %d\n",errno,ret);
341
342
343
344
345
346
347
348
349
350
351
		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
352
   {
353
		return -1;
julien.delange's avatar
julien.delange committed
354
   }
355
356
	
	ret = ioctl(chan->fd,BRM_SET_STD,&arg);
julien.delange's avatar
julien.delange committed
357
358
359
360
361

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


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

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

393
394
395
396
397
398
399
400
401
402
403
	/* 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
404
   {
405
		return -1;
julien.delange's avatar
julien.delange committed
406
   }
407
408
	
	ret = ioctl(chan->fd,BRM_RX_BLOCK,&arg);
julien.delange's avatar
julien.delange committed
409
410
411
412

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

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

419
420
421
422
423
	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
424

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

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

432
433
434
435
436
437
438
439
	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
440
   {
441
		return -1;
julien.delange's avatar
julien.delange committed
442
   }
443
444
	
	ret = ioctl(chan->fd,BRM_SET_BCE,&arg);
julien.delange's avatar
julien.delange committed
445
446
447
448

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

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

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

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

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

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

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

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

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

	__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);

519
	/* print data */			
julien.delange's avatar
julien.delange committed
520
521
522
523
524
525
526
527
528
	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]);
529
530
531
		}
	}
	if ( k > 0 )
julien.delange's avatar
julien.delange committed
532
533
534
   {
		__DEBUGMSG("\n");
   }
535
536
}

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

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

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

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

julien.delange's avatar
julien.delange committed
547
548
549
	__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);
550
551
552
	
	/* no message data in BC transmit commands */
	if ( tr )
julien.delange's avatar
julien.delange committed
553
   {
554
		return;
julien.delange's avatar
julien.delange committed
555
   }
556
	
julien.delange's avatar
julien.delange committed
557
	__DEBUGMSG("         ");
558
559
	
	/* print data */			
julien.delange's avatar
julien.delange committed
560
561
562
563
564
565
566
567
568
	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]);
569
570
		}
	}
julien.delange's avatar
julien.delange committed
571

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

#endif