int __po_hi_c_driver_drvmgr_grspw_sender
(const __po_hi_task_id task_id,
 const __po_hi_port_t port)
{
   int len = -1;
   int i;
   int ts;

   __po_hi_c_spacewire_conf_t* sender_conf;
   __po_hi_c_spacewire_conf_t* receiver_conf;

   __po_hi_local_port_t    local_port;
   __po_hi_request_t*      request;
   __po_hi_port_t          destination_port;

   __po_hi_device_id       dev_id;

   struct route_entry route; /* Routing table */

   dev_id = __po_hi_get_device_from_port (port);

   if (dev_id == invalid_device_id) {
      __PO_HI_DEBUG_DEBUG ("[GRSPW] Invalid device id for sending\n");
      return __PO_HI_UNAVAILABLE;
   }

   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
        ("[GRSPW SPACEWIRE] Send output task %d, port %d : no value to send\n",
         task_id, port);
      return __PO_HI_SUCCESS;
   }

   destination_port = __po_hi_gqueue_get_destination (task_id, local_port, 0);

   __po_hi_msg_reallocate (&__po_hi_c_driver_drvmgr_grspw_sender_msg);

   request->port = destination_port;

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

   __po_hi_marshall_request
     (request, &__po_hi_c_driver_drvmgr_grspw_sender_msg);

   len = -1;

   memset(&route, 0, sizeof(route));
   route.dstadr[0]= 1;

#if __PO_HI_DEBUG_LEVEL >= __PO_HI_DEBUG_LEVEL_DEBUG
      __PO_HI_DEBUG_DEBUG ("Message content: |0x");
      for (ts = 0 ; ts < __PO_HI_MESSAGES_MAX_SIZE ; ts++) {
        __PO_HI_DEBUG_DEBUG
          ("%x", __po_hi_c_driver_drvmgr_grspw_sender_msg.content[ts]);
      }
      __PO_HI_DEBUG_DEBUG ("|\n");
#endif

   len = grspw_sending
     (0, // XXX hardcoded
      &route,
      __po_hi_c_driver_drvmgr_grspw_sender_msg.content,
      __PO_HI_MESSAGES_MAX_SIZE);

   if (len < 0) {
      __PO_HI_DEBUG_CRITICAL (" [GRSPW SPACEWIRE] failed !\n");
   } else
     if((0 <= len)&(len < __PO_HI_MESSAGES_MAX_SIZE)) {
      __PO_HI_DEBUG_CRITICAL (" [GRSPW SPACEWIRE] Unable write !\n");
     } else {
       __PO_HI_DEBUG_DEBUG (" [GRSPW SPACEWIRE] Send OK !\n");
   }

   request->port = __PO_HI_GQUEUE_INVALID_PORT;

   return __PO_HI_SUCCESS;
}
Esempio n. 2
0
int __po_hi_c_driver_spacewire_rasta_sender (const __po_hi_task_id task_id, const __po_hi_port_t port)
{
   int len = -1;
   int i;

   __po_hi_c_spacewire_conf_t* sender_conf;
   __po_hi_c_spacewire_conf_t* receiver_conf;

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

   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 ("[RASTA SPACEWIRE] Send output task %d, port %d : no value to send\n", task_id, port);
      return __PO_HI_SUCCESS;
   }

   destination_port     = __po_hi_gqueue_get_destination (task_id, local_port, 0);

   __po_hi_msg_reallocate (&__po_hi_c_driver_spacewire_rasta_sender_msg);

   request->port = destination_port;

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

   __po_hi_marshall_request (request, &__po_hi_c_driver_spacewire_rasta_sender_msg);

   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);
   }
   
   if (len < 0)
   {
      __PO_HI_DEBUG_DEBUG (" failed !\n");
   }
   else
   {
      __PO_HI_DEBUG_DEBUG (" OK !\n");
   }

   request->port = __PO_HI_GQUEUE_INVALID_PORT;

   return 1;
}
int  __po_hi_c_driver_eth_leon_sender (__po_hi_task_id task, __po_hi_port_t port)
{
   int                        len;
   int                        size_to_write;
   int                        optval = 0;
   socklen_t                  optlen = 0;

   unsigned long* swap_pointer;
   unsigned long swap_value;

   __po_hi_device_id          associated_device;
   __po_hi_local_port_t       local_port;
   __po_hi_request_t*         request;
   __po_hi_port_t             destination_port;
   __po_hi_protocol_t         protocol_id;
   __po_hi_protocol_conf_t*   protocol_conf;

   local_port              = __po_hi_get_local_port_from_global_port (port);
   request                 = __po_hi_gqueue_get_most_recent_value (task, local_port);
   destination_port        = __po_hi_gqueue_get_destination (task, local_port, 0);
   associated_device       = __po_hi_get_device_from_port (destination_port);
   protocol_id             = __po_hi_transport_get_protocol (port, destination_port);
   protocol_conf           = __po_hi_transport_get_protocol_configuration (protocol_id);

   if (request->port == -1)
   {

#ifdef __PO_HI_DEBUG
      __DEBUGMSG (" [DRIVER SOCKETS] No data to write on port %d\n", port);
#endif
      return __PO_HI_ERROR_TRANSPORT_SEND;
   }

   if (nodes[associated_device].socket == -1 )
   {
#ifdef __PO_HI_DEBUG
      __DEBUGMSG (" [DRIVER SOCKETS] Invalid socket for port-id %d, device-id %d\n", destination_port, associated_device);
#endif
      return __PO_HI_ERROR_TRANSPORT_SEND;
   }

   /*
    * After sending the entity identifier, we send the message which
    * contains the request.
    */

   size_to_write = __PO_HI_MESSAGES_MAX_SIZE;

   if (getsockopt (nodes[associated_device].socket, SOL_SOCKET, SO_ERROR, &optval, &optlen) == -1)
   {
      __DEBUGMSG (" [error getsockopt() in file %s, line%d ]\n", __FILE__, __LINE__);
      close (nodes[associated_device].socket);
      nodes[associated_device].socket = -1;
      return __PO_HI_ERROR_TRANSPORT_SEND;
   }

   if (optval != 0)
   {
      __DEBUGMSG (" [error getsockopt() return code in file %s, line%d ]\n", __FILE__, __LINE__);
      close (nodes[associated_device].socket);
      nodes[associated_device].socket = -1;
      return __PO_HI_ERROR_TRANSPORT_SEND;
   }

   /* Ignore SIGPIPE to be able to recover from errors instead of crashing the node */

   if (signal (SIGPIPE, SIG_IGN) == SIG_ERR)
   {
      __DEBUGMSG (" [error signal() return code in file %s, line%d ]\n", __FILE__, __LINE__);
      close (nodes[associated_device].socket);
      nodes[associated_device].socket = -1;
      return __PO_HI_ERROR_TRANSPORT_SEND;
   }

   switch (protocol_id)
   {
#ifdef __PO_HI_USE_PROTOCOL_MYPROTOCOL_I
      case virtual_bus_myprotocol_i:
      {
         size_to_write = sizeof (int);
         int datawritten;
         protocol_conf->marshaller(request, &datawritten, &size_to_write);
         len = write (nodes[associated_device].socket, &datawritten, size_to_write);

         if (len != size_to_write)
         {
            __DEBUGMSG (" [error write() length in file %s, line%d ]\n", __FILE__, __LINE__);
            close (nodes[associated_device].socket);
            nodes[associated_device].socket = -1;
            return __PO_HI_ERROR_TRANSPORT_SEND;
         }
         break;
      }
#endif
      default:
      {
         request->port = destination_port;
         __po_hi_msg_reallocate (&__po_hi_c_driver_eth_leon_sender_msg);
         __po_hi_marshall_request (request, &__po_hi_c_driver_eth_leon_sender_msg);

#ifdef __PO_HI_DEBUG
         __po_hi_messages_debug (&__po_hi_c_driver_eth_leon_sender_msg);
#endif

         swap_pointer  = (unsigned long*) &__po_hi_c_driver_eth_leon_sender_msg.content[0];
         swap_value    = *swap_pointer;
         *swap_pointer = __po_hi_swap_byte (swap_value);
         len = write (nodes[associated_device].socket, &(__po_hi_c_driver_eth_leon_sender_msg.content), size_to_write);

         if (len != size_to_write)
         {
            __DEBUGMSG (" [error write() length in file %s, line%d ]\n", __FILE__, __LINE__);
            close (nodes[associated_device].socket);
            nodes[associated_device].socket = -1;
            return __PO_HI_ERROR_TRANSPORT_SEND;
         }

         request->port = __PO_HI_GQUEUE_INVALID_PORT;
         break;
      }
   }

   return __PO_HI_SUCCESS;
}
Esempio n. 4
0
int __po_hi_driver_sockets_send (__po_hi_task_id task_id,
                                 __po_hi_port_t port)
{
   int                        len;
   int                        size_to_write;
#ifndef _WIN32
   int                        optval = 0;
   socklen_t                  optlen = 0;
#else
   char FAR                   optval = 0;
   int FAR                    optlen = 0;
#endif
   __po_hi_device_id          remote_device;
   __po_hi_device_id          local_device;
   __po_hi_local_port_t       local_port;
   __po_hi_request_t*         request;
   __po_hi_port_t             destination_port;
   __po_hi_protocol_t         protocol_id;
   __po_hi_protocol_conf_t*   protocol_conf;
   __po_hi_monitor_status_t   device_status;

   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);
   local_device            = __po_hi_get_device_from_port (port);
   remote_device           = __po_hi_get_device_from_port (destination_port);
   protocol_id             = __po_hi_transport_get_protocol (port, destination_port);
   protocol_conf           = __po_hi_transport_get_protocol_configuration (protocol_id);


   __DEBUGMSG ("[DRIVER SOCKETS] Try to write from task=%d, port=%d, local_device=%d, remote device=%d, socket=%d\n", task_id, port, local_device, remote_device, __po_hi_c_sockets_write_sockets[remote_device]);
   if (request->port == -1)
   {
      __DEBUGMSG (" [DRIVER SOCKETS] No data to write on port %d\n", port);
      return __PO_HI_ERROR_TRANSPORT_SEND;
   }

#if __PO_HI_MONITOR_ENABLED
   if (__po_hi_monitor_get_status_device (local_device, &device_status) != __PO_HI_SUCCESS)
   {
      __DEBUGMSG ("[DRIVER SOCKETS] Cannot get the status of device %d\n", local_device);
      return __PO_HI_ERROR_TRANSPORT_SEND;
   }

   if (device_status.status != po_hi_monitor_status_ok)
   {
      __DEBUGMSG ("[DRIVER SOCKETS] Device has a problem and is not able to process the request, aborting (device-id=%d, status= %d)\n", local_device, device_status);
      return __PO_HI_ERROR_TRANSPORT_SEND;
   }
#endif

   if (__po_hi_c_sockets_write_sockets[remote_device] == -1 )
   {
#ifdef __PO_HI_DEBUG
      __DEBUGMSG (" [DRIVER SOCKETS] Invalid socket for port-id %d, device-id %d\n", destination_port, remote_device);
#endif
      return __PO_HI_ERROR_TRANSPORT_SEND;
   }

   /*
    * After sending the entity identifier, we send the message which
    * contains the request.
    */

   size_to_write = __PO_HI_MESSAGES_MAX_SIZE;

   if (getsockopt (__po_hi_c_sockets_write_sockets[remote_device], SOL_SOCKET, SO_ERROR, &optval, &optlen) == -1)
   {
      __DEBUGMSG (" [error getsockopt() in file %s, line%d ]\n", __FILE__, __LINE__);
      close (__po_hi_c_sockets_write_sockets[remote_device]);
      __po_hi_c_sockets_write_sockets[remote_device] = -1;
      return __PO_HI_ERROR_TRANSPORT_SEND;
   }

   if (optval != 0)
   {
      __DEBUGMSG (" [error getsockopt() return code in file %s, line%d ]\n", __FILE__, __LINE__);
      close (__po_hi_c_sockets_write_sockets[remote_device]);
      __po_hi_c_sockets_write_sockets[remote_device] = -1;
      return __PO_HI_ERROR_TRANSPORT_SEND;
   }

#ifndef _WIN32
   /*
    * Ignore SIGPIPE to be able to recover from
    * errors instead of crashing the node
    */

   if (signal (SIGPIPE, SIG_IGN) == SIG_ERR)
   {
      __DEBUGMSG (" [error signal() return code in file %s, line%d ]\n", __FILE__, __LINE__);
      close (__po_hi_c_sockets_write_sockets[remote_device]);
      __po_hi_c_sockets_write_sockets[remote_device] = -1;
      return __PO_HI_ERROR_TRANSPORT_SEND;
   }
#endif

   switch (protocol_id)
   {
#ifdef __PO_HI_USE_PROTOCOL_MYPROTOCOL_I
      case virtual_bus_myprotocol_i:
      {
         size_to_write = sizeof (int);
         int datawritten;
         protocol_conf->marshaller(request, &datawritten, &size_to_write);
#ifdef _WIN32
         len = send (__po_hi_c_sockets_write_sockets[remote_device], &datawritten, size_to_write, 0);
#else
         len = write (__po_hi_c_sockets_write_sockets[remote_device], &datawritten, size_to_write);
#endif

         if (len != size_to_write)
         {
            __DEBUGMSG (" [error write() length in file %s, line%d ]\n", __FILE__, __LINE__);
            close (__po_hi_c_sockets_write_sockets[remote_device]);
            __po_hi_c_sockets_write_sockets[remote_device] = -1;
            return __PO_HI_ERROR_TRANSPORT_SEND;
         }
         break;
      }
#endif
      default:
      {
         request->port = destination_port;
         __po_hi_msg_reallocate (&__po_hi_c_sockets_send_msg);
         __po_hi_marshall_request (request, &__po_hi_c_sockets_send_msg);

#ifdef __PO_HI_DEBUG
         __po_hi_messages_debug (&__po_hi_c_sockets_send_msg[remote_device]);
#endif
         if (__po_hi_c_sockets_write_sockets[remote_device] != -1)
         {

#ifdef _WIN32
            len = send (__po_hi_c_sockets_write_sockets[remote_device], (char*) &(__po_hi_c_sockets_send_msg.content), size_to_write, 0);
#else
            len = write (__po_hi_c_sockets_write_sockets[remote_device], &(__po_hi_c_sockets_send_msg.content), size_to_write);
#endif

            if (len != size_to_write)
            {

#if __PO_HI_MONITOR_ENABLED
               __po_hi_monitor_report_failure_device (remote_device, po_hi_monitor_failure_value);
#endif

               __DEBUGMSG (" [error write() length in file %s, line%d ]\n", __FILE__, __LINE__);
               close (__po_hi_c_sockets_write_sockets[remote_device]);
               __po_hi_c_sockets_write_sockets[remote_device] = -1;
               return __PO_HI_ERROR_TRANSPORT_SEND;
            }
         }

         request->port = __PO_HI_GQUEUE_INVALID_PORT;
         break;
      }
   }

   return __PO_HI_SUCCESS;
}