po_hi_driver_rasta_spacewire.c 8.44 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) 2010-2014 ESA & ISAE.
9
10
11
12
13
14
15
 */

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

#ifdef __PO_HI_NEED_DRIVER_SPACEWIRE_RASTA

16
#include <activity.h>
17
#include <marshallers.h>
18
#include <deployment.h>
19
20
21
22
23

#include <po_hi_debug.h>
#include <po_hi_transport.h>
#include <po_hi_gqueue.h>
#include <po_hi_messages.h>
jdelange's avatar
jdelange committed
24
25
26
#include <po_hi_returns.h>


27
#include <drivers/po_hi_rtems_utils.h>
28
#include <drivers/po_hi_driver_rasta_spacewire.h>
29
#include <drivers/po_hi_driver_rasta_common.h>
30

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

38
39
40
41

#include <rtems/bspIo.h>
#include <ambapp.h>

42
43
44
45
46
47
48
49
#include <pci.h>
#include <rasta.h>
#include <grspw_rasta.h>
#include <apbuart_rasta.h>
/* Rasta includes from GAISLER drivers */

#define __PO_HI_DRIVER_SPACEWIRE_RASTA_DEVICE "/dev/grspwrasta0"

50
51
52
 __po_hi_request_t   __po_hi_c_driver_spacewire_rasta_request;
__po_hi_msg_t        __po_hi_c_driver_spacewire_rasta_poller_msg;
int                  po_hi_c_driver_rasta_spacewire_fd[__PO_HI_NB_DEVICES];
jdelange's avatar
jdelange committed
53
char                 __po_hi_c_driver_rasta_spacewire_sndbuf[__PO_HI_MESSAGES_MAX_SIZE + 1];
54

55
void __po_hi_c_driver_spacewire_rasta_poller (const __po_hi_device_id dev_id)
56
{
57
58
   int len;
   int j;
59
   int ts;
60

61

62
   __PO_HI_DEBUG_DEBUG ("[RASTA SPACEWIRE] Hello, i'm the spacewire poller !\n");
63

64
   __po_hi_msg_reallocate (&__po_hi_c_driver_spacewire_rasta_poller_msg);
65
66

   len = read (po_hi_c_driver_rasta_spacewire_fd[dev_id],
67
               &__po_hi_c_driver_spacewire_rasta_poller_msg.content[0],
68
               __PO_HI_MESSAGES_MAX_SIZE);
69

70
   __PO_HI_DEBUG_DEBUG ("[RASTA SPACEWIRE] Poller received a message, len=%d\n", len);
71
72
   if (len < 0)
   {
73
      __PO_HI_DEBUG_DEBUG ("[RASTA SPACEWIRE] Error while reading\n");
74
75
76
77
   }
   else
   {

78
79
      __PO_HI_DEBUG_DEBUG ("Message content: |0x");
      for (ts = 0 ; ts < __PO_HI_MESSAGES_MAX_SIZE ; ts++)
80
      {
81
         __PO_HI_DEBUG_DEBUG ("%x", __po_hi_c_driver_spacewire_rasta_poller_msg.content[ts]);
82
      }
83
      __PO_HI_DEBUG_DEBUG ("|\n");
84
85


86
87
      __po_hi_c_driver_spacewire_rasta_poller_msg.length = __PO_HI_MESSAGES_MAX_SIZE;
      __po_hi_unmarshall_request (&__po_hi_c_driver_spacewire_rasta_request, &__po_hi_c_driver_spacewire_rasta_poller_msg);
88

89
90
      __PO_HI_DEBUG_DEBUG ("[RASTA SPACEWIRE] Destination port: %d\n", __po_hi_c_driver_spacewire_rasta_request.port);
      __po_hi_main_deliver (&__po_hi_c_driver_spacewire_rasta_request);
91
   }
92
93
}

94
95
96
97
98
99
100

extern rtems_isr __po_hi_rasta_interrupt_handler (rtems_vector_number v);
extern unsigned int __po_hi_driver_rasta_bar0, __po_hi_driver_rasta_bar1;

void __po_hi_rasta_interrrupt_register(void *handler, int irqno, void *arg);
extern amba_confarea_type* __po_hi_driver_rasta_common_get_bus ();

101
void __po_hi_c_driver_spacewire_rasta_init (__po_hi_device_id id)
102
{
103
   unsigned int node_addr;
jdelange's avatar
jdelange committed
104
105
   __po_hi_c_spacewire_conf_t* drv_conf;

106
   drv_conf = (__po_hi_c_spacewire_conf_t*) __po_hi_get_device_configuration (id);
107

jdelange's avatar
jdelange committed
108
   node_addr = drv_conf->nodeaddr;
109

jdelange's avatar
jdelange committed
110
   __PO_HI_DEBUG_INFO ("[RASTA SPACEWIRE] Init, node address=%d\n", node_addr);
111

112
   __po_hi_c_driver_rasta_common_init ();
113

jdelange's avatar
jdelange committed
114
115
   __po_hi_transport_set_sending_func (id, __po_hi_c_driver_spacewire_rasta_sender);

116
   __PO_HI_DEBUG_DEBUG ("[RASTA SPACEWIRE] Open spacewire device %s ...", drv_conf->devname);
117

118
   po_hi_c_driver_rasta_spacewire_fd[id] = open (drv_conf->devname, O_RDWR);
119

120
   if (po_hi_c_driver_rasta_spacewire_fd[id] < 0)
121
   {
122
      __PO_HI_DEBUG_DEBUG (" ERROR !\n");
123
      return;
124
125
   }

126
   __PO_HI_DEBUG_DEBUG (" OK !\n");
127

128
   __PO_HI_DEBUG_DEBUG ("[RASTA SPACEWIRE] Configure spacewire device node address = %d ...", node_addr);
129

130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
/*
   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd, SPACEWIRE_IOCTRL_SET_COREFREQ, 0);                 // Core frequency in KHz (0 = full speed)
   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd, SPACEWIRE_IOCTRL_SET_CLKDIV, 2);                   // Clock division factor
   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd, SPACEWIRE_IOCTRL_SET_RMAPEN, 1);                   // No RMAP
   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd, SPACEWIRE_IOCTRL_SET_NODEADDR, node_addr);         // Not necessary
   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1);                  // Blocking read
   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd, SPACEWIRE_IOCTRL_SET_CHECK_RMAP, 0);               // Do not check RMAP CRC
   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd, SPACEWIRE_IOCTRL_SET_RM_PROT_ID, 0);               // Do not remove protocol id
   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 0);                  // Non blocking write
   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1);          // Blocking write if full
   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd, SPACEWIRE_IOCTRL_SET_PROMISCUOUS, 1);              // Receive from any source
*/
   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd[id],SPACEWIRE_IOCTRL_SET_COREFREQ,30000); 
   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd[id],SPACEWIRE_IOCTRL_SET_NODEADDR, node_addr);
   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd[id],SPACEWIRE_IOCTRL_SET_RXBLOCK,0);
   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd[id],SPACEWIRE_IOCTRL_SET_TXBLOCK,0);
   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd[id],SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL,1);
   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd[id],SPACEWIRE_IOCTRL_SET_RM_PROT_ID,0);

   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd[id], SPACEWIRE_IOCTRL_SET_PROMISCUOUS, 1);              // Receive from any source


   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd[id],SPACEWIRE_IOCTRL_START,2000);
153
154
}

155
156
157

__po_hi_msg_t           __po_hi_c_driver_spacewire_rasta_sender_msg;

158
159
int __po_hi_c_driver_spacewire_rasta_sender (const __po_hi_task_id task_id, const __po_hi_port_t port)
{
160
161
   int len = -1;
   int i;
jdelange's avatar
jdelange committed
162
163
164
165

   __po_hi_c_spacewire_conf_t* sender_conf;
   __po_hi_c_spacewire_conf_t* receiver_conf;

166
167
168
169
170
171
172
173
174
175
176
177
178
   __po_hi_local_port_t    local_port;
   __po_hi_request_t*      request;
   __po_hi_port_t          destination_port;

   __po_hi_device_id       dev_id;

   dev_id = __po_hi_get_device_from_port (port);

   if (dev_id == invalid_device_id)
   {
      __PO_HI_DEBUG_DEBUG ("[RASTA SPW] Invalid device id for sending\n");
      return __PO_HI_UNAVAILABLE;
   }
179
180
181
182
183

   local_port = __po_hi_get_local_port_from_global_port (port);

   request = __po_hi_gqueue_get_most_recent_value (task_id, local_port);

jdelange's avatar
jdelange committed
184
185
186
187
188
189
   if (request->port == -1)
   {
      __PO_HI_DEBUG_DEBUG ("[RASTA SPACEWIRE] Send output task %d, port %d : no value to send\n", task_id, port);
      return __PO_HI_SUCCESS;
   }

190
191
   destination_port     = __po_hi_gqueue_get_destination (task_id, local_port, 0);

192
   __po_hi_msg_reallocate (&__po_hi_c_driver_spacewire_rasta_sender_msg);
193
194
195

   request->port = destination_port;

jdelange's avatar
jdelange committed
196
197
   sender_conf = (__po_hi_c_spacewire_conf_t*) __po_hi_get_device_configuration (dev_id);
   receiver_conf = (__po_hi_c_spacewire_conf_t*) __po_hi_get_device_configuration (__po_hi_get_device_from_port (destination_port));
198

jdelange's avatar
jdelange committed
199
   __po_hi_marshall_request (request, &__po_hi_c_driver_spacewire_rasta_sender_msg);
200

jdelange's avatar
jdelange committed
201
202
203
204
205
206
207
208
209
210
211
212
   len = -1;
   if (sender_conf->use_router == TRUE)
   {
      __po_hi_c_driver_rasta_spacewire_sndbuf[0] = receiver_conf->nodeaddr;
      memcpy (&__po_hi_c_driver_rasta_spacewire_sndbuf[1], __po_hi_c_driver_spacewire_rasta_sender_msg.content , __PO_HI_MESSAGES_MAX_SIZE);
      len = write (po_hi_c_driver_rasta_spacewire_fd[dev_id], __po_hi_c_driver_rasta_spacewire_sndbuf, __PO_HI_MESSAGES_MAX_SIZE + 1);
   }
   else
   {
      len = write (po_hi_c_driver_rasta_spacewire_fd[dev_id], __po_hi_c_driver_spacewire_rasta_sender_msg.content, __PO_HI_MESSAGES_MAX_SIZE);
   }
   
213
214
   if (len < 0)
   {
215
      __PO_HI_DEBUG_DEBUG (" failed !\n");
216
217
218
   }
   else
   {
219
      __PO_HI_DEBUG_DEBUG (" OK !\n");
220
   }
jdelange's avatar
jdelange committed
221
222
223

   request->port = __PO_HI_GQUEUE_INVALID_PORT;

224
225
226
227
   return 1;
}

#endif /* __PO_HI_NEED_DRIVER_SPACEWIRE_RASTA */