po_hi_driver_rasta_1553.c 8.89 KB
Newer Older
julien.delange's avatar
julien.delange committed
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
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
/*
 * 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
 */


#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

__po_hi_c_driver_rasta_1553_brm_t po_hi_c_driver_1553_rasta_fd;

void __po_hi_c_driver_1553_rasta_poller (void)
{
   int ret;
   int msglen;

   __po_hi_msg_t msg;
   __po_hi_request_t request;

   struct rt_msg msgs[__PO_HI_NEED_DRIVER_1553_RASTA_MSG_CNT];

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

   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;

   memcpy (&msg, msgs[0].data, __PO_HI_MESSAGES_MAX_SIZE);

   __po_hi_unmarshall_request (&request, &msg);

   __DEBUGMSG ("[RASTA 1553] Destination port: %d\n", request.port);

   __po_hi_main_deliver (&request);
}

int __po_hi_c_driver_1553_rasta_sender (const __po_hi_task_id task_id, const __po_hi_port_t port)
{
   int ts;
   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_msg_t msg;
   __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);

   __po_hi_msg_reallocate (&msg);

   request->port = destination_port;

   __po_hi_marshall_request (request, &msg);

   memcpy (msgs[0].data, &msg, __PO_HI_MESSAGES_MAX_SIZE);

   msgs[0].desc = 32;

   __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_brmlib_rt_send (po_hi_c_driver_1553_rasta_fd, msgs) != 1 )
   {
      printf("[RASTA 1553] Error when sending data\n");
   }

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

   for (ts = 0 ; ts < __PO_HI_MESSAGES_MAX_SIZE ; ts++)
   {
      __DEBUGMSG ("%x", msg.content[ts]);
   }
   __DEBUGMSG ("\n");

   return 1;
}


julien.delange's avatar
julien.delange committed
138
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
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
{
   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
177
void __po_hi_c_driver_1553_rasta_controller ()
julien.delange's avatar
julien.delange committed
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
{
   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
197
198
199

   /* 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
200
   {
julien.delange's avatar
julien.delange committed
201
202
203
204
      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
205

julien.delange's avatar
julien.delange committed
206
      for (k = 0; k < 9; k++)
julien.delange's avatar
julien.delange committed
207
      {
julien.delange's avatar
julien.delange committed
208
         cmd_list[j-1].data[k] = 0;
julien.delange's avatar
julien.delange committed
209
210
      }

julien.delange's avatar
julien.delange committed
211
212
213
214
215
      /* 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
216

julien.delange's avatar
julien.delange committed
217
218
219
220
221
222
223
224
225
226
227
   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
228
229
      for (k = 0; k < 9; k++)
      {
julien.delange's avatar
julien.delange committed
230
         result_list[j-1].data[k] = 0;
julien.delange's avatar
julien.delange committed
231
      }
julien.delange's avatar
julien.delange committed
232
   }
julien.delange's avatar
julien.delange committed
233

julien.delange's avatar
julien.delange committed
234
235
   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
236

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

julien.delange's avatar
julien.delange committed
239
//   __DEBUGMSG("[RASTA 1553] Start CMD list processing.\n"); 
julien.delange's avatar
julien.delange committed
240
241
   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
242
243
      return;
   }
julien.delange's avatar
julien.delange committed
244

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

julien.delange's avatar
julien.delange committed
248
249
250
251
252
   /* 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
253
      {
julien.delange's avatar
julien.delange committed
254
         result_list[j-1].data[k] = 0;
julien.delange's avatar
julien.delange committed
255
      }
julien.delange's avatar
julien.delange committed
256
   }
julien.delange's avatar
julien.delange committed
257

julien.delange's avatar
julien.delange committed
258
259
260
261
   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
262

julien.delange's avatar
julien.delange committed
263
264
265
266
   /* print the data that was received */
   j=1;
   while( !(result_list[j-1].ctrl & BC_EOL) )
   {
julien.delange's avatar
julien.delange committed
267
//      __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]);
julien.delange's avatar
julien.delange committed
268
      /* print data */			
julien.delange's avatar
julien.delange committed
269
 /*     for (k = 0; k < result_list[j-1].wc; k++)
julien.delange's avatar
julien.delange committed
270
      {
julien.delange's avatar
julien.delange committed
271
         if ( isalnum(result_list[j-1].data[k]) )
julien.delange's avatar
julien.delange committed
272
         {
julien.delange's avatar
julien.delange committed
273
274
275
276
277
            __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
278
279
         }
      }
julien.delange's avatar
julien.delange committed
280
      __DEBUGMSG("\n");
julien.delange's avatar
julien.delange committed
281
      */
julien.delange's avatar
julien.delange committed
282
      j++;
julien.delange's avatar
julien.delange committed
283
   }
julien.delange's avatar
julien.delange committed
284

julien.delange's avatar
julien.delange committed
285
286
287
288
}



julien.delange's avatar
julien.delange committed
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
void __po_hi_c_driver_1553_rasta_init_terminal (__po_hi_device_id id)
{
   __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;
   }

   __po_hi_c_driver_1553_rasta_brmlib_set_mode (po_hi_c_driver_1553_rasta_fd,BRM_MODE_RT);
}


void __po_hi_c_driver_1553_rasta_init_controller (__po_hi_device_id id)
julien.delange's avatar
julien.delange committed
315
316
317
{
   __DEBUGMSG ("[RASTA 1553] Init\n");
   init_pci();
julien.delange's avatar
julien.delange committed
318
   __DEBUGMSG ("[RASTA 1553] Initializing RASTA (rasta_register()) ...\n");
julien.delange's avatar
julien.delange committed
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
   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
335
336
337
   /* Set BC mode */
   __DEBUGMSG("[RASTA 1553] Task1: Setting BC mode\n");
   __po_hi_c_driver_1553_rasta_brmlib_set_mode (po_hi_c_driver_1553_rasta_fd,BRM_MODE_BC);
julien.delange's avatar
julien.delange committed
338
339
340
}


julien.delange's avatar
julien.delange committed
341

julien.delange's avatar
julien.delange committed
342
343
344
345
#endif