po_hi_driver_rasta_1553.c 11.9 KB
Newer Older
julien.delange's avatar
julien.delange committed
1
2
3
4
5
/*
 * 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.
 *
yoogx's avatar
yoogx committed
6
 * For more informations, please visit http://taste.tuxfamily.org/wiki
jhugues's avatar
jhugues committed
7
 *
yoogx's avatar
yoogx committed
8
 * Copyright (C) 2010-2014 ESA & ISAE.
julien.delange's avatar
julien.delange committed
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
35
36
37
38
39
40
41
42
43
 */


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

#ifdef __PO_HI_NEED_DRIVER_1553_RASTA


#include <activity.h>
#include <marshallers.h>

#include <po_hi_debug.h>
#include <po_hi_transport.h>
#include <po_hi_gqueue.h>
#include <po_hi_messages.h>
#include <po_hi_utils.h>
#include <drivers/po_hi_rtems_utils.h>
#include <drivers/po_hi_driver_rasta_1553.h>
#include <drivers/po_hi_driver_rasta_1553_brmlib.h>

#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <fcntl.h>
/* POSIX-style files */

#include <pci.h>
#include <rasta.h>
/* Rasta includes from GAISLER drivers */

#define __PO_HI_DRIVER_RASTA_1553_DEVICE "/dev/brmrasta0"

#define __PO_HI_NEED_DRIVER_1553_RASTA_MSG_CNT 4

44
45
46
__po_hi_c_driver_rasta_1553_brm_t         po_hi_c_driver_1553_rasta_fd;
__po_hi_request_t                         po_hi_c_driver_1553_rasta_request;
__po_hi_msg_t                             __po_hi_c_driver_1553_rasta_terminal_poller_msg;
julien.delange's avatar
julien.delange committed
47

48
void __po_hi_c_driver_1553_rasta_terminal_poller (void)
julien.delange's avatar
julien.delange committed
49
{
50
51
52
   int            ret;
   int            msglen;
   struct rt_msg  msgs[__PO_HI_NEED_DRIVER_1553_RASTA_MSG_CNT];
julien.delange's avatar
julien.delange committed
53
54
55
56
57
58
59
60
61
62
63
64
65

   __DEBUGMSG ("[RASTA 1553] Hello, i'm the poller !\n");

   __po_hi_c_driver_1553_rasta_brmlib_set_block(po_hi_c_driver_1553_rasta_fd,1,0);

   ret = __po_hi_c_driver_1553_rasta_brmlib_rt_recv (po_hi_c_driver_1553_rasta_fd,msgs);

   if ( ret <= 0 )
   {
      __DEBUGMSG ("[RASTA 1553] Error when receive, return code=%d\n",ret); 
      return;
   }

66
67
   __DEBUGMSG ("[RASTA 1553] Poller received %d messages\n",ret); 

julien.delange's avatar
julien.delange committed
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
   if ( msgs[0].desc >= 32 )
   {
      __DEBUGMSG ("[RASTA 1553] Received message desc >= 32\n");
      return;
   }

   __DEBUGMSG ("[RASTA 1553] ---------------  RT MESSAGE: -------------------\n");

   if (msgs[0].miw & (1<<7))
   {
      printf("[RASTA 1553] Message error, desc: %d, miw:%x\n", msgs[0].desc, msgs[0].miw);
   }

   __DEBUGMSG ("[RASTA 1553] desc: %d, miw: %x, time: %x\n",msgs[0].desc, msgs[0].miw, msgs[0].time);

   msglen = (msgs[0].miw >> 11) & 0x1f;

85
   memcpy (&__po_hi_c_driver_1553_rasta_terminal_poller_msg, msgs[0].data, __PO_HI_MESSAGES_MAX_SIZE);
86
87
88
89
   {
#include <stdio.h>
   printf("received (length=%d): 0x", msglen);
   int i;
90
   uint32_t* toto = (uint32_t*) &__po_hi_c_driver_1553_rasta_terminal_poller_msg;
91
92
93
94
95
96
97
98
   for (i = 0 ; i < __PO_HI_MESSAGES_MAX_SIZE ; i++)
   {
      printf("%x", toto[i]);
   }

   printf("\n");
   }

julien.delange's avatar
julien.delange committed
99

100
   __po_hi_unmarshall_request (&po_hi_c_driver_1553_rasta_request, &__po_hi_c_driver_1553_rasta_terminal_poller_msg);
julien.delange's avatar
julien.delange committed
101

102
   __DEBUGMSG ("[RASTA 1553] Destination port: %d\n", po_hi_c_driver_1553_rasta_request.port);
julien.delange's avatar
julien.delange committed
103

104
   __po_hi_main_deliver (&po_hi_c_driver_1553_rasta_request);
julien.delange's avatar
julien.delange committed
105
106
}

107
108

__po_hi_msg_t __po_hi_c_driver_1553_rasta_sender_terminal_msg;
109
int __po_hi_c_driver_1553_rasta_sender_terminal (const __po_hi_task_id task_id, const __po_hi_port_t port)
julien.delange's avatar
julien.delange committed
110
111
{
   int ts;
112
   int ret;
julien.delange's avatar
julien.delange committed
113
114
115
116
117
118
119
120
121
122
123
124
125
   struct rt_msg msgs[__PO_HI_NEED_DRIVER_1553_RASTA_MSG_CNT];

   __po_hi_local_port_t local_port;
   __po_hi_request_t* request;
   __po_hi_port_t destination_port;


   local_port = __po_hi_get_local_port_from_global_port (port);

   request = __po_hi_gqueue_get_most_recent_value (task_id, local_port);

   destination_port     = __po_hi_gqueue_get_destination (task_id, local_port, 0);

126
   __po_hi_msg_reallocate (&__po_hi_c_driver_1553_rasta_sender_terminal_msg);
julien.delange's avatar
julien.delange committed
127
128
129

   request->port = destination_port;

130
   __po_hi_marshall_request (request, &__po_hi_c_driver_1553_rasta_sender_terminal_msg);
julien.delange's avatar
julien.delange committed
131

132
   memcpy (msgs[0].data, &__po_hi_c_driver_1553_rasta_sender_terminal_msg, __PO_HI_MESSAGES_MAX_SIZE);
julien.delange's avatar
julien.delange committed
133

134
135
136
   msgs[0].miw = 0;
   msgs[0].miw |= (__PO_HI_MESSAGES_MAX_SIZE / 8) << 11;
   msgs[0].desc = 2;
julien.delange's avatar
julien.delange committed
137
138
139

   __po_hi_c_driver_1553_rasta_brmlib_set_block (po_hi_c_driver_1553_rasta_fd, 1, 0);

140
141
   ret = __po_hi_c_driver_1553_rasta_brmlib_rt_send (po_hi_c_driver_1553_rasta_fd, msgs);
   if (ret != 1)
julien.delange's avatar
julien.delange committed
142
   {
143
144
      printf("[RASTA 1553] Error when sending data, return code=%d\n", ret);
      return;
julien.delange's avatar
julien.delange committed
145
146
147
148
149
150
   }

   __DEBUGMSG  ("[RASTA 1553] Message sent: 0x");

   for (ts = 0 ; ts < __PO_HI_MESSAGES_MAX_SIZE ; ts++)
   {
151
      __DEBUGMSG ("%x", __po_hi_c_driver_1553_rasta_sender_terminal_msg.content[ts]);
julien.delange's avatar
julien.delange committed
152
153
154
155
156
157
158
   }
   __DEBUGMSG ("\n");

   return 1;
}


159
__po_hi_msg_t __po_hi_c_driver_1553_rasta_sender_controller_msg;
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
int __po_hi_c_driver_1553_rasta_sender_controller (const __po_hi_task_id task_id, const __po_hi_port_t port)
{
   struct bc_msg msgs[2];

   msgs[0].rtaddr[0] = 2;
   msgs[0].subaddr[0] = 1;
   msgs[0].wc = __PO_HI_MESSAGES_MAX_SIZE / 8;
   msgs[0].ctrl = 0;
//   msgs[0].ctrl |= BC_TR;
   msgs[0].ctrl |= BC_BUSA;

   msgs[1].ctrl |= BC_EOL;   /* end of list */

   __po_hi_local_port_t local_port;
   __po_hi_request_t* request;
   __po_hi_port_t destination_port;


   local_port = __po_hi_get_local_port_from_global_port (port);

   request = __po_hi_gqueue_get_most_recent_value (task_id, local_port);

   destination_port     = __po_hi_gqueue_get_destination (task_id, local_port, 0);

184
   __po_hi_msg_reallocate (&__po_hi_c_driver_1553_rasta_sender_controller_msg);
185
186
187

   request->port = destination_port;

188
   __po_hi_marshall_request (request, &__po_hi_c_driver_1553_rasta_sender_controller_msg);
189

190
   memcpy (msgs[0].data, &__po_hi_c_driver_1553_rasta_sender_controller_msg, __PO_HI_MESSAGES_MAX_SIZE);
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219

   /*
   {
#include <stdio.h>
   printf("sent : 0x");
   int i;
   uint32_t* toto = (uint32_t*) &msg;
   for (i = 0 ; i < __PO_HI_MESSAGES_MAX_SIZE ; i++)
   {
      printf("%x", toto[i]);
   }

   printf("\n");
   }
   */
   
   __po_hi_c_driver_1553_rasta_brmlib_set_block (po_hi_c_driver_1553_rasta_fd, 1, 0);

   if ( __po_hi_c_driver_1553_rasta_proccess_list(po_hi_c_driver_1553_rasta_fd,msgs,0) )
   {
      __DEBUGMSG("[RASTA 1553] CANNOT PROCESS LIST\n");
      return 0;
   }

   return 1;
}



julien.delange's avatar
julien.delange committed
220
int __po_hi_c_driver_1553_rasta_proccess_list (__po_hi_c_driver_rasta_1553_brm_t chan, struct bc_msg *list, int test)
julien.delange's avatar
julien.delange committed
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
{
   int j,ret;

   if ( __po_hi_c_driver_1553_rasta_brmlib_bc_dolist(chan, list) )
   {
      __DEBUGMSG("[RASTA 1553] LIST EXECUTION INIT FAILED\n");
      return -1;
   }

   /* Blocks until done */
   __DEBUGMSG("[RASTA 1553] Waiting until list processed\n"); 
   if ( (ret=__po_hi_c_driver_1553_rasta_brmlib_bc_dolist_wait(chan)) < 0 )
   {
      __DEBUGMSG("[RASTA 1553] LIST EXECUTION DONE FAILED\n");
      return -1;
   }

   if ( ret != 1 ) 
   {
      /* not done */
      __DEBUGMSG("[RASTA 1553] LIST NOT PROCESSED\n");
      return -1;
   }

   /* done */
   __DEBUGMSG("[RASTA 1553] List processed.\n");

   for (j = 1; j <= __PO_HI_NEED_DRIVER_1553_RASTA_MSG_CNT; j++) 
   {
      if (list[j-1].ctrl & BC_BAME)
      {
         __DEBUGMSG("[RASTA 1553] Error msg %d, %x: BAME\n", j, list[j-1].ctrl);
      }
   }

   return 0;
}

julien.delange's avatar
julien.delange committed
259
void __po_hi_c_driver_1553_rasta_controller ()
julien.delange's avatar
julien.delange committed
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
{
   struct bc_msg cmd_list[__PO_HI_NEED_DRIVER_1553_RASTA_MSG_CNT+1];
   struct bc_msg result_list[__PO_HI_NEED_DRIVER_1553_RASTA_MSG_CNT+1];
   int j,k;

   __DEBUGMSG("[RASTA 1553] Starting task 1: BC mode\n");

   /* before starting set up
    *  ¤ blocking mode
    *  ¤ BC mode
    */


   /* total blocking mode */
   __DEBUGMSG("[RASTA 1553] Task1: Setting TX/RX blocking mode\n"); 
   __po_hi_c_driver_1553_rasta_brmlib_set_block(po_hi_c_driver_1553_rasta_fd,1,1);

   __DEBUGMSG("[RASTA 1553] Setting up command list.\n");

julien.delange's avatar
julien.delange committed
279
280
281

   /* Set up messages to RT receive subaddresses */
   for (j = 1; j <= __PO_HI_NEED_DRIVER_1553_RASTA_MSG_CNT; j++) 
julien.delange's avatar
julien.delange committed
282
   {
julien.delange's avatar
julien.delange committed
283
284
285
286
      cmd_list[j-1].rtaddr[0]  = 1;
      cmd_list[j-1].subaddr[0] = j;
      cmd_list[j-1].wc         = 8;
      cmd_list[j-1].ctrl       = BC_BUSA; /* RT receive on bus a */
julien.delange's avatar
julien.delange committed
287

julien.delange's avatar
julien.delange committed
288
      for (k = 0; k < 9; k++)
julien.delange's avatar
julien.delange committed
289
      {
julien.delange's avatar
julien.delange committed
290
         cmd_list[j-1].data[k] = 0;
julien.delange's avatar
julien.delange committed
291
292
      }

julien.delange's avatar
julien.delange committed
293
294
295
296
297
      /* message input */
      cmd_list[j-1].data[1] = 'G';
      cmd_list[j-1].data[2] = 'R';
      cmd_list[j-1].data[3] = (j-1)+7;
   }
julien.delange's avatar
julien.delange committed
298

julien.delange's avatar
julien.delange committed
299
300
301
302
303
304
305
306
307
308
309
   cmd_list[__PO_HI_NEED_DRIVER_1553_RASTA_MSG_CNT-1].wc++;
   cmd_list[__PO_HI_NEED_DRIVER_1553_RASTA_MSG_CNT].ctrl |= BC_EOL;   /* end of list */

   /* Set up RT transmit sub addresses (request RTs to send answer) */
   for (j = 1; j <= __PO_HI_NEED_DRIVER_1553_RASTA_MSG_CNT; j++) 
   {
      result_list[j-1].rtaddr[0]  = 1;
      result_list[j-1].subaddr[0] = j;
      result_list[j-1].wc         = 8;
      result_list[j-1].ctrl       = BC_BUSA | BC_TR; /* RT transmit on bus a */		
      /* clear data */
julien.delange's avatar
julien.delange committed
310
311
      for (k = 0; k < 9; k++)
      {
julien.delange's avatar
julien.delange committed
312
         result_list[j-1].data[k] = 0;
julien.delange's avatar
julien.delange committed
313
      }
julien.delange's avatar
julien.delange committed
314
   }
julien.delange's avatar
julien.delange committed
315

julien.delange's avatar
julien.delange committed
316
317
   result_list[__PO_HI_NEED_DRIVER_1553_RASTA_MSG_CNT-1].wc++;
   result_list[__PO_HI_NEED_DRIVER_1553_RASTA_MSG_CNT].ctrl |= BC_EOL;   /* end of list */
julien.delange's avatar
julien.delange committed
318

julien.delange's avatar
julien.delange committed
319
//   __DEBUGMSG("[RASTA 1553] -------------  BC: START LIST EXECUTION -------------\n");
julien.delange's avatar
julien.delange committed
320

julien.delange's avatar
julien.delange committed
321
//   __DEBUGMSG("[RASTA 1553] Start CMD list processing.\n"); 
julien.delange's avatar
julien.delange committed
322
323
   if ( __po_hi_c_driver_1553_rasta_proccess_list(po_hi_c_driver_1553_rasta_fd,cmd_list,0) )
   {
julien.delange's avatar
julien.delange committed
324
325
      return;
   }
julien.delange's avatar
julien.delange committed
326

julien.delange's avatar
julien.delange committed
327
328
   __DEBUGMSG("[RASTA 1553] -------------  BC: START LIST EXECUTION -------------\n");
   __DEBUGMSG("[RASTA 1553] Start RESULT list processing.\n"); 
julien.delange's avatar
julien.delange committed
329

julien.delange's avatar
julien.delange committed
330
331
332
333
334
   /* Clear data that input will overwrite */
   for (j = 1; j <= __PO_HI_NEED_DRIVER_1553_RASTA_MSG_CNT; j++) 
   {
      /* clear data */
      for (k = 0; k < 8; k++)
julien.delange's avatar
julien.delange committed
335
      {
julien.delange's avatar
julien.delange committed
336
         result_list[j-1].data[k] = 0;
julien.delange's avatar
julien.delange committed
337
      }
julien.delange's avatar
julien.delange committed
338
   }
julien.delange's avatar
julien.delange committed
339

julien.delange's avatar
julien.delange committed
340
341
342
343
   if ( __po_hi_c_driver_1553_rasta_proccess_list(po_hi_c_driver_1553_rasta_fd,result_list,1) )
   {
      return;
   }
julien.delange's avatar
julien.delange committed
344

julien.delange's avatar
julien.delange committed
345
   /* print the data that was received */
346
   /*
julien.delange's avatar
julien.delange committed
347
348
349
   j=1;
   while( !(result_list[j-1].ctrl & BC_EOL) )
   {
350
351
      __DEBUGMSG("[RASTA 1553] Response to message %d: (len: %d, tsw1: %x, tsw2: %x)\n  ",j,result_list[j-1].wc,result_list[j-1].tsw[0],result_list[j-1].tsw[1]);
     for (k = 0; k < result_list[j-1].wc; k++)
julien.delange's avatar
julien.delange committed
352
      {
julien.delange's avatar
julien.delange committed
353
         if ( isalnum(result_list[j-1].data[k]) )
julien.delange's avatar
julien.delange committed
354
         {
julien.delange's avatar
julien.delange committed
355
356
357
358
359
            __DEBUGMSG("[RASTA 1553] 0x%x (%c)",result_list[j-1].data[k],result_list[j-1].data[k]);
         }
         else
         {
            __DEBUGMSG("[RASTA 1553] 0x%x (.)",result_list[j-1].data[k]);
julien.delange's avatar
julien.delange committed
360
361
         }
      }
julien.delange's avatar
julien.delange committed
362
      __DEBUGMSG("\n");
363
      
julien.delange's avatar
julien.delange committed
364
      j++;
julien.delange's avatar
julien.delange committed
365
   }
366
   */
julien.delange's avatar
julien.delange committed
367

julien.delange's avatar
julien.delange committed
368
369
370
371
}



julien.delange's avatar
julien.delange committed
372
373
void __po_hi_c_driver_1553_rasta_init_terminal (__po_hi_device_id id)
{
374
375
   int ret;

julien.delange's avatar
julien.delange committed
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
   __DEBUGMSG ("[RASTA 1553] Init\n");
   init_pci();
   __DEBUGMSG ("[RASTA 1553] Initializing RASTA (rasta_register()) ...\n");
   if (rasta_register())
   {
      __DEBUGMSG(" ERROR !\n");
      return;
   }

   __DEBUGMSG(" OK !\n");

   po_hi_c_driver_1553_rasta_fd = __po_hi_c_driver_1553_rasta_brmlib_open (__PO_HI_DRIVER_RASTA_1553_DEVICE);

   if (po_hi_c_driver_1553_rasta_fd < 0)
   {
      __DEBUGMSG ("[RASTA 1553] Unable to open 1553 device\n");
      return;
   }

395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
   ret = __po_hi_c_driver_1553_rasta_brmlib_set_mode (po_hi_c_driver_1553_rasta_fd,BRM_MODE_RT);

   if (ret != 0)
   {
      __DEBUGMSG ("Error setting address, return=%d\n", ret);
      return;
   }

   ret = __po_hi_c_driver_1553_rasta_brmlib_set_rt_addr (po_hi_c_driver_1553_rasta_fd, 2);

   if (ret != 0)
   {
      __DEBUGMSG ("Error setting address, return=%d\n", ret);
      return;
   }
   return;
julien.delange's avatar
julien.delange committed
411
412
413
414
}


void __po_hi_c_driver_1553_rasta_init_controller (__po_hi_device_id id)
julien.delange's avatar
julien.delange committed
415
{
416
417
   int ret;

julien.delange's avatar
julien.delange committed
418
419
   __DEBUGMSG ("[RASTA 1553] Init\n");
   init_pci();
julien.delange's avatar
julien.delange committed
420
   __DEBUGMSG ("[RASTA 1553] Initializing RASTA (rasta_register()) ...\n");
julien.delange's avatar
julien.delange committed
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
   if (rasta_register())
   {
      __DEBUGMSG(" ERROR !\n");
      return;
   }

   __DEBUGMSG(" OK !\n");

   po_hi_c_driver_1553_rasta_fd = __po_hi_c_driver_1553_rasta_brmlib_open (__PO_HI_DRIVER_RASTA_1553_DEVICE);

   if (po_hi_c_driver_1553_rasta_fd < 0)
   {
      __DEBUGMSG ("[RASTA 1553] Unable to open 1553 device\n");
      return;
   }

julien.delange's avatar
julien.delange committed
437
   /* Set BC mode */
438
439
440
441
442
443
444
445
446
   __DEBUGMSG("[RASTA 1553] Setting BC mode\n");

   ret = __po_hi_c_driver_1553_rasta_brmlib_set_mode (po_hi_c_driver_1553_rasta_fd,BRM_MODE_BC);

   if (ret != 0)
   {
      __DEBUGMSG ("Error setting BC mode, return=%d\n", ret);
   }

julien.delange's avatar
julien.delange committed
447
448
449
}


julien.delange's avatar
julien.delange committed
450

julien.delange's avatar
julien.delange committed
451
#endif