po_hi_driver_usbbrick_spacewire.c 9.29 KB
Newer Older
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) 2011-2014 ESA & ISAE.
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
 */

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

#ifdef __PO_HI_NEED_DRIVER_SPACEWIRE_USB_BRICK

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

#include <po_hi_debug.h>
#include <po_hi_transport.h>
#include <po_hi_gqueue.h>
#include <po_hi_messages.h>
#include <po_hi_returns.h>

#include <drivers/po_hi_driver_usbbrick_spacewire.h>

#include <stdlib.h>
29
#include <string.h>
30
31
32
33
34
35
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <fcntl.h>
/* POSIX-style files */

36
37
38
39
40
41
#include "star_dundee_types.h"

#define SPW_MAX_PACKET_SIZE     8192
#define SPW_SEND_PACKET_SIZE    512

int                  __po_hi_c_driver_usb_brick_fd[__PO_HI_NB_DEVICES];
42
int                  __po_hi_c_driver_usb_brick_port[__PO_HI_NB_DEVICES];
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
star_device_handle   __po_hi_c_driver_usb_brick_star_device[__PO_HI_NB_DEVICES];

int __po_hi_driver_usbbrick_spw_init  (star_device_handle *phDevice,   /* Pointer to device handler */
                                       unsigned char num,              /* Device id to init */
                                       U32 frequency,
                                       U32 divider      /* frequency and divider */
)
{
        /* Open the SpaceWire device */
        if (!USBSpaceWire_Open(phDevice, num)) 
        {
                return 0xffffffff;
        }

        /* Get header mode for restoring at closure */
        if (USBSpaceWire_IsHeaderModeEnabled(*phDevice)) 
        {
                USBSpaceWire_EnableHeaderMode(phDevice, 0);
        }

        /* Get network mode for restoring at closure */
        if (USBSpaceWire_IsNetworkModeEnabled(*phDevice)) 
        {
                USBSpaceWire_EnableNetworkMode(*phDevice, 0);
        }

        /* Device must be RMAP for the following configuration */
        /* Mandatory configuration for StarDundee device: do not modify */
        CFGSpaceWire_EnableRMAP(1);
        CFGSpaceWire_SetRMAPDestinationKey(0x20);
        CFGSpaceWire_AddrStackPush(0);
        CFGSpaceWire_AddrStackPush(254);
        CFGSpaceWire_RetAddrStackPush(254);

        /* Receive on all ports */
        USBSpaceWire_RegisterReceiveOnAllPorts(*phDevice);

        if (CFGSpaceWire_SetAsInterface(*phDevice, 1, 0) != CFG_TRANSFER_SUCCESS) 
        {
82
           __DEBUGMSG(stderr, "Couldn't set the device as an interface.\n");
83
84
85
86
87
88
89
        }

        /* Setting the speed */
        CFGSpaceWire_SetBrickBaseTransmitRate(*phDevice, frequency, divider, 0xff);

        return 0;
}
90

91
92
93
__po_hi_request_t __po_hi_c_driver_spw_usb_brick_request;
__po_hi_msg_t     __po_hi_c_driver_spw_usb_brick_poller_msg;

94
95
void __po_hi_c_driver_spw_usb_brick_poller (const __po_hi_device_id dev_id)
{
96
97
98
99
100
101
102
103
104
105
106
107
   int n;
   int ts;

   unsigned long* swap_pointer;
   unsigned long swap_value;

   USB_SPACEWIRE_STATUS status;
   USB_SPACEWIRE_PACKET_PROPERTIES props;
   USB_SPACEWIRE_EOP_TYPE eop;
   USB_SPACEWIRE_ID id;


108
   __PO_HI_DEBUG_DEBUG ("[USB-SPW] Hello, i'm the spacewire poller on the brick, must read %d bytes!\n", __PO_HI_MESSAGES_MAX_SIZE);
109

110
   __po_hi_msg_reallocate (&__po_hi_c_driver_spw_usb_brick_poller_msg);
111

112
   status = USBSpaceWire_ReadPackets(__po_hi_c_driver_usb_brick_star_device[dev_id], &__po_hi_c_driver_spw_usb_brick_poller_msg.content[0], __PO_HI_MESSAGES_MAX_SIZE, 1, 1, &props, &id);
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
138
139
140
   if (status != TRANSFER_SUCCESS)
   {
      __PO_HI_DEBUG_DEBUG ("[USB-SPW] Read error, status=%d!\n", status);
   }

   eop = props.eop;

   USBSpaceWire_FreeRead(__po_hi_c_driver_usb_brick_star_device[dev_id], id);

   n = props.len;

   if (n == -1)
   {
      __PO_HI_DEBUG_DEBUG ("[USB-SPẄ] Cannot read !\n");
      return;
   }

   if (n == 0)
   {
      return;
   }

   if (n != __PO_HI_MESSAGES_MAX_SIZE)
   {
      __PO_HI_DEBUG_CRITICAL ("[USB-SPW] Inconsistent received message size (received %d bytes)!\n", n);
      return;
   }

141
   __PO_HI_DEBUG_DEBUG ("[USB-SPW] read() on %d returns %d\n", __po_hi_c_driver_usb_brick_star_device[dev_id], n);
142
143
144
145
146
147
148



   __PO_HI_DEBUG_DEBUG  ("[USB-SPW] Message: 0x");

   for (ts = 0 ; ts < __PO_HI_MESSAGES_MAX_SIZE ; ts++)
   {
149
      __PO_HI_DEBUG_DEBUG ("%x", __po_hi_c_driver_spw_usb_brick_poller_msg.content[ts]);
150
151
   }
   __PO_HI_DEBUG_DEBUG ("\n");
152
   swap_pointer  = (unsigned long*) &__po_hi_c_driver_spw_usb_brick_poller_msg.content[0];
153
154
155
   swap_value    = *swap_pointer;
   *swap_pointer = __po_hi_swap_byte (swap_value);

156
   __po_hi_c_driver_spw_usb_brick_poller_msg.length = n;
157
158

   __PO_HI_DEBUG_DEBUG ("[USB-SPW] Message after swapped port: 0x");
159
   for (ts = 0 ; ts < __po_hi_c_driver_spw_usb_brick_poller_msg.length ; ts++)
160
   {
161
        __PO_HI_DEBUG_DEBUG ("%x", __po_hi_c_driver_spw_usb_brick_poller_msg.content[ts]);
162
163
164
   }
   __PO_HI_DEBUG_DEBUG ("\n");

165
   __PO_HI_DEBUG_DEBUG ("[USB-SPW] Received: %s\n", __po_hi_c_driver_spw_usb_brick_poller_msg.content);
166

167
   __po_hi_unmarshall_request (&__po_hi_c_driver_spw_usb_brick_request, &__po_hi_c_driver_spw_usb_brick_poller_msg);
168

169
   if (__po_hi_c_driver_spw_usb_brick_request.port > __PO_HI_NB_PORTS)
170
171
172
173
174
   {
      __PO_HI_DEBUG_DEBUG ("[USB-SPW] Invalid port number !\n");
      return;
   }

175
176
   __PO_HI_DEBUG_DEBUG ("[USB-SPW] Destination port: %d\n", __po_hi_c_driver_spw_usb_brick_request.port);
   __po_hi_main_deliver (&__po_hi_c_driver_spw_usb_brick_request);
177
178


179
180
181
182
}

void __po_hi_c_driver_spw_usb_brick_init (__po_hi_device_id id)
{
183
   int i;
184
185
186
187
   __po_hi_c_spacewire_conf_t* drv_conf;

   drv_conf = (__po_hi_c_spacewire_conf_t*) __po_hi_get_device_configuration (id);

188
   /* Get the first device connected */
189
190
   __po_hi_c_driver_usb_brick_fd[id]   = USBSpaceWire_ListDevices();

jdelange's avatar
jdelange committed
191
192
193

   __po_hi_transport_set_sending_func (id, __po_hi_c_driver_spw_usb_brick_sender);

194
   __po_hi_c_driver_usb_brick_port[id] = 1;
195
196
   if (strncmp (drv_conf->devname, "node2", 5) == 0)
   {
197
      __po_hi_c_driver_usb_brick_port[id] = 1;
198
199
   }

200
201
202
203
204
205
206
207
208
209
210
211
212
   for (i = 0; i < 32; i++) 
   {
      if (__po_hi_c_driver_usb_brick_fd[id] == (unsigned long)(1 << i)) 
      {
         __po_hi_c_driver_usb_brick_fd[id] = i;
         break;
      }
   }

   /* Initialize the device at 100 Mbps */
   if (__po_hi_driver_usbbrick_spw_init(&__po_hi_c_driver_usb_brick_star_device[id],__po_hi_c_driver_usb_brick_fd[id] , CFG_BRK_CLK_200_MHZ, CFG_BRK_DVDR_2) == -1) 
   {
      __PO_HI_DEBUG_DEBUG ("[USB-BRICK] SpaceWire device initialisation error.\n");
213
      return;
214
   }
215
216

   __PO_HI_DEBUG_DEBUG ("[USB-BRICK] SpaceWire device initialisation complete, fd=%d\n", __po_hi_c_driver_usb_brick_fd[id] );
217
218
}

219
220
221
222


__po_hi_msg_t           __po_hi_c_driver_spw_usb_brick_sender_msg;

223
224
int __po_hi_c_driver_spw_usb_brick_sender (const __po_hi_task_id task_id, const __po_hi_port_t port)
{
225
226
227
228
229
230
231
232
233
234
235
236
237
238
   int                     n;
   int                     ts;

   uint8_t buf[__PO_HI_MESSAGES_MAX_SIZE+1];

   unsigned long* swap_pointer;
   unsigned long swap_value;
   __po_hi_local_port_t    local_port;
   __po_hi_request_t*      request;
   __po_hi_port_t          destination_port;
   __po_hi_device_id       dev_id;

   USB_SPACEWIRE_STATUS    status;
   USB_SPACEWIRE_ID        id;
239

240
   dev_id = __po_hi_get_device_from_port (port);
241

242
243
244
245
246
   if (dev_id == invalid_device_id)
   {
      __PO_HI_DEBUG_DEBUG ("[USB-SPW] Invalid device id for sending\n");
      return __PO_HI_UNAVAILABLE;
   }
247

248
249
250
251
252
253
254
255
256
257
258
259
   local_port = __po_hi_get_local_port_from_global_port (port);

   request = __po_hi_gqueue_get_most_recent_value (task_id, local_port);

   if (request->port == -1)
   {
      __PO_HI_DEBUG_DEBUG ("[USB-SPW] Send output task %d, port %d (local_port=%d): no value to send\n", task_id, port, local_port);
      return __PO_HI_SUCCESS;
   }

   destination_port     = __po_hi_gqueue_get_destination (task_id, local_port, 0);

260
   __po_hi_msg_reallocate (&__po_hi_c_driver_spw_usb_brick_sender_msg);
261
262
263
264

   request->port = destination_port;
   __PO_HI_DEBUG_DEBUG ("[USB-SPW] Destination port= %d, send through device %d (fd=%d)\n", destination_port, dev_id, __po_hi_c_driver_usb_brick_fd[dev_id]);

265
266
   __po_hi_marshall_request (request, &__po_hi_c_driver_spw_usb_brick_sender_msg);
   swap_pointer  = (unsigned long*) &__po_hi_c_driver_spw_usb_brick_sender_msg.content[0];
267
268
   swap_value    = *swap_pointer;
   *swap_pointer = __po_hi_swap_byte (swap_value);
269
270
271

   buf[0] = __po_hi_c_driver_usb_brick_port[dev_id];

272
273
   __PO_HI_DEBUG_DEBUG ("[USB-SPW] write() send on port %d\n", __po_hi_c_driver_usb_brick_port[dev_id]);

274
   memcpy (&buf[1], __po_hi_c_driver_spw_usb_brick_sender_msg.content, __PO_HI_MESSAGES_MAX_SIZE);
275
276
277
   if ((status = USBSpaceWire_SendPacket(__po_hi_c_driver_usb_brick_star_device[dev_id], buf, __PO_HI_MESSAGES_MAX_SIZE + 1, 1, &id)) != TRANSFER_SUCCESS) 
   {
      __PO_HI_DEBUG_DEBUG("[USB-SPW] Write error: %d.\n", status);
278
      return 0;
279
280
281
282
283
   }

   __PO_HI_DEBUG_DEBUG ("Sent: |0x");
   for (ts = 0 ; ts < __PO_HI_MESSAGES_MAX_SIZE ; ts++)
   {
284
      __PO_HI_DEBUG_DEBUG ("%x", __po_hi_c_driver_spw_usb_brick_sender_msg.content[ts]);
285
286
287
288
289
290
   }
   __PO_HI_DEBUG_DEBUG ("|\n");


   USBSpaceWire_FreeSend (__po_hi_c_driver_usb_brick_star_device[dev_id], id);

291
   __PO_HI_DEBUG_DEBUG ("[USB-SPW] write() using star device %d returns %d\n", __po_hi_c_driver_usb_brick_star_device[dev_id - 1], status);
292
293
294
295
296

   request->port = __PO_HI_GQUEUE_INVALID_PORT;

   return 1;
}
297

298
#endif