po_hi_driver_rasta_spacewire.c 6.92 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13
/*
 * 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_SPACEWIRE_RASTA

14
#include <activity.h>
15
#include <marshallers.h>
16
#include <deployment.h>
17 18 19 20 21

#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
22 23 24
#include <po_hi_returns.h>


25
#include <drivers/po_hi_rtems_utils.h>
26
#include <drivers/po_hi_driver_rasta_spacewire.h>
27
#include <drivers/po_hi_driver_rasta_common.h>
28

29
#include <stdlib.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

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

40 41 42 43 44 45 46 47
#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"

48 49 50
static unsigned char po_hi_c_driver_spacewire_rxpkt[__PO_HI_DRIVER_RASTA_SPACEWIRE_PKTSIZE * __PO_HI_DRIVER_RASTA_SPACEWIRE_RXPKT_BUF];
static __po_hi_c_driver_spacewire_pkt_hdr_t po_hi_c_driver_spacewire_txpkts[1];

51 52
int po_hi_c_driver_rasta_spacewire_fd;

53
void po_hi_c_driver_rasta_spacewire_init_pkt(__po_hi_c_driver_spacewire_pkt_hdr_t *p, __po_hi_port_t destination_port)
54
{
jdelange's avatar
jdelange committed
55 56 57 58 59
   unsigned int dst_node_addr;
   __po_hi_c_spacewire_conf_t* dst_conf;

   dst_conf = (__po_hi_c_spacewire_conf_t*) __po_hi_get_device_configuration 
               (__po_hi_get_device_from_port (destination_port));
60

jdelange's avatar
jdelange committed
61 62 63 64 65
   dst_node_addr = dst_conf->nodeaddr;

   p->addr = dst_node_addr;

  __PO_HI_DEBUG_DEBUG ("[RASTA SPACEWIRE] Init packet with destination address address %d !\n", p->addr);
66

67 68 69 70 71 72 73
  p->protid = 50;
  p->dummy = 0x01;
  p->channel = 0x01;
  memset (p->data, '\0', __PO_HI_DRIVER_RASTA_SPACEWIRE_PKTSIZE);
}


74 75
void __po_hi_c_driver_spacewire_rasta_poller (void)
{
76 77
   int len;
   int j;
78 79 80

   __po_hi_msg_t msg;
   __po_hi_request_t request;
81

82
   __PO_HI_DEBUG_DEBUG ("[RASTA SPACEWIRE] Hello, i'm the spacewire poller !\n");
83 84 85 86 87

   len = read (po_hi_c_driver_rasta_spacewire_fd,
               &po_hi_c_driver_spacewire_rxpkt[0],
               __PO_HI_DRIVER_RASTA_SPACEWIRE_PKTSIZE * __PO_HI_DRIVER_RASTA_SPACEWIRE_RXPKT_BUF);

88
   __PO_HI_DEBUG_DEBUG ("[RASTA SPACEWIRE] Poller reveived a message !\n");
89 90
   if (len < 0)
   {
91
      __PO_HI_DEBUG_DEBUG ("[RASTA SPACEWIRE] Error while reading\n");
92 93 94 95
   }
   else
   {

96 97 98 99 100 101 102 103 104 105 106 107 108 109
      /* skip first 2bytes (vchan and dummy) */
      if ( (po_hi_c_driver_spacewire_rxpkt[0]==1) && (po_hi_c_driver_spacewire_rxpkt[1]==1) )
      {
         j=2; /* strip virtual channel protocol, non-ssspw device */
      }
      else
      {
         j=0; /* hardware uses virtual channel protocol, hw already stripped it */
      }

      memcpy (&msg.content, &po_hi_c_driver_spacewire_rxpkt[j], __PO_HI_MESSAGES_MAX_SIZE + j);

      __po_hi_unmarshall_request (&request, &msg);

110
      __PO_HI_DEBUG_DEBUG ("[RASTA SPACEWIRE] Destination port: %d\n", request.port);
111 112
      __po_hi_main_deliver (&request);
   }
113 114
}

115 116 117 118 119 120 121

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

122
void __po_hi_c_driver_spacewire_rasta_init (__po_hi_device_id id)
123
{
124
   unsigned int node_addr;
jdelange's avatar
jdelange committed
125 126 127 128
   __po_hi_c_spacewire_conf_t* drv_conf;

   drv_conf = (__po_hi_c_spacewire_conf_t*) __po_hi_get_device_configuration 
               (id);
129

jdelange's avatar
jdelange committed
130
   node_addr = drv_conf->nodeaddr;
131

jdelange's avatar
jdelange committed
132 133

   __PO_HI_DEBUG_INFO ("[RASTA SPACEWIRE] Init, node address=%d\n", node_addr);
134

135
   __po_hi_c_driver_rasta_common_init ();
136

137 138 139 140 141 142
    /* provide the spacewire driver with AMBA Plug&Play
     * info so that it can find the GRSPW cores.
     */
   /*
    grspw_rasta_int_reg=__po_hi_rasta_interrrupt_register;
    if ( grspw_rasta_register(__po_hi_driver_rasta_common_get_bus (),__po_hi_driver_rasta_bar1) ){
143
      __PO_HI_DEBUG_CRITICAL ("Failed to register RASTA GRSPW driver\n\r");
144 145 146 147
      return;
    }
    */

148
   __PO_HI_DEBUG_DEBUG ("[RASTA SPACEWIRE] Open spacewire device ...");
149 150 151 152 153

   po_hi_c_driver_rasta_spacewire_fd = open (__PO_HI_DRIVER_SPACEWIRE_RASTA_DEVICE, O_RDWR);

   if (po_hi_c_driver_rasta_spacewire_fd < 0)
   {
154
      __PO_HI_DEBUG_DEBUG (" ERROR !\n");
155
      return;
156 157
   }

158
   __PO_HI_DEBUG_DEBUG (" OK !\n");
159

160
   __PO_HI_DEBUG_DEBUG ("[RASTA SPACEWIRE] Configure spacewire device node address = %d ...", node_addr);
161 162 163 164

   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd,SPACEWIRE_IOCTRL_SET_COREFREQ,30000); 
   /* make driver calculate timings from 30MHz spacewire clock */
   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd,SPACEWIRE_IOCTRL_SET_NODEADDR, node_addr);
165
   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd,SPACEWIRE_IOCTRL_SET_RXBLOCK,0);
166 167 168 169 170
   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd,SPACEWIRE_IOCTRL_SET_TXBLOCK,0);
   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd,SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL,1);
   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd,SPACEWIRE_IOCTRL_SET_RM_PROT_ID,1);
   /* remove protocol id */
   __PO_HI_DRIVERS_RTEMS_UTILS_IOCTL(po_hi_c_driver_rasta_spacewire_fd,SPACEWIRE_IOCTRL_START,2000);
171 172 173 174
}

int __po_hi_c_driver_spacewire_rasta_sender (const __po_hi_task_id task_id, const __po_hi_port_t port)
{
175 176
   int len = -1;
   int i;
177 178 179 180 181 182 183 184 185
   __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);

jdelange's avatar
jdelange committed
186 187 188 189 190 191
   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;
   }

192 193 194 195 196 197 198
   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);
199

200
   __PO_HI_DEBUG_DEBUG ("[RASTA SPACEWIRE] Send packet destination port = %d ...", destination_port);
201 202 203

   for(i=0; i<1; i++)
   {
204
      po_hi_c_driver_rasta_spacewire_init_pkt (&po_hi_c_driver_spacewire_txpkts[i], destination_port);
205

206
      __PO_HI_DEBUG_DEBUG ("destination node = %d ...", po_hi_c_driver_spacewire_txpkts[i].addr);
207 208
   }

209
   memcpy (po_hi_c_driver_spacewire_txpkts[0].data, &msg, __PO_HI_MESSAGES_MAX_SIZE);
210

211
   len = write (po_hi_c_driver_rasta_spacewire_fd, po_hi_c_driver_spacewire_txpkts, __PO_HI_DRIVER_RASTA_SPACEWIRE_PKTSIZE + 4);
212 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 228 229
   return 1;
}

#endif /* __PO_HI_NEED_DRIVER_SPACEWIRE_RASTA */