renamings... redo the request and messages part... also now there will be two threads...

[frescor/fna.git] / src_frescan / frescan.c

diff --git a/src_frescan/frescan.c b/src_frescan/frescan.c
index 8f1b9c485bf23e34f7a1618191182f47f2deeec9..4084c8ca93880710b455cdbd71c6024262ee7c52 100644 (file)
--- a/src_frescan/frescan.c
+++ b/src_frescan/frescan.c
@@ -34,14 +34,17 @@
 #include "frescan_debug.h"     // DEBUG
 #include "frescan_id.h"        // frescan_id_set_field, frescan_id_get_field
 #include "frescan_hw_buffer.h" // frescan_hw_buffer_update
-#include "frescan_reply_objects.h"     // frescan_replyobjects_init
+#include "frescan_bwres_robjs.h"     // frescan_replyobjects_init
 #include "frescan_servers_replenishments.h" // frescan_replenishments_xxx
+#include "frescan_packets.h"
 
 static int frescan_hook_frame_recv (const struct can_chip_t *chip,
                                     struct can_frame_t *frame);
 
 static int frescan_hook_frame_sent(const struct can_chip_t *chip);
 
+static int frescan_hook_frame_aborted(const struct can_chip_t *chip);
+
 /**
  * frescan_init - initializes the network and the internal structures
  *
@@ -99,7 +102,7 @@ int frescan_init(frescan_init_params_t *params)
                 return -1;
         }
 
-        DEBUG(FRESCAN_INIT_ENABLE_DEBUG, "set tx and rx hooks\n");
+        DEBUG(FRESCAN_INIT_ENABLE_DEBUG, "set tx, rx, abort hooks\n");
 
         ret = ioctl(fd, CAN_IOCTL_SET_TX_HOOK, frescan_hook_frame_sent);
         if (ret == -1) {
@@ -115,6 +118,13 @@ int frescan_init(frescan_init_params_t *params)
                 return -1;
         }
 
+        ret = ioctl(fd, CAN_IOCTL_SET_AB_HOOK, frescan_hook_frame_aborted);
+        if (ret == -1) {
+                ERROR ("ioctl CAN_IOCTL_SET_AB_HOOK failed /dev/can%u\n",
+                       params->net);
+                return -1;
+        }
+
         DEBUG(FRESCAN_INIT_ENABLE_DEBUG, "init the rest of modules\n");
 
         ret = frescan_data_init(fd, params);
@@ -147,12 +157,6 @@ int frescan_init(frescan_init_params_t *params)
                 return -1;
         }
 
-        ret = frescan_replyobjects_init(FRESCAN_REPLY_OBJECTS_MX_CEILING);
-        if (ret != 0) {
-                ERROR("could not initialize the reply objects\n");
-                return -1;
-        }
-
         return 0;
 }
 
@@ -328,10 +332,7 @@ int frescan_recv(const frescan_recv_params_t *params,
         pqueue = the_networks[params->net].queues.rx_channel_queues
                                                              [params->channel];
 
-        FRESCAN_ACQUIRE_LOCK(&the_networks[params->net].lock);
         ret = frescan_pqueue_dequeue(pqueue, &head, prio, blocking);
-        FRESCAN_RELEASE_LOCK(&the_networks[params->net].lock);
-
         if (ret != 0) {
                 ERROR ("could not dequeue packet\n");
                 return -1;
@@ -363,9 +364,11 @@ int frescan_recv(const frescan_recv_params_t *params,
                        packet->frame->dlc);
                 *recv_bytes += packet->frame->dlc;
 
+                *from = (frescan_node_t)frescan_id_get_field(packet->frame->id,
+                                                             FRESCAN_FIELD_SRC);
                 DEBUG(FRESCAN_RECV_ENABLE_DEBUG,
-                      "ID Packet, dlc: %u, frame pool pos: %u\n",
-                      packet->frame->dlc, packet->frame->pool_pos);
+                      "ID Packet, dlc: %u, frame pool pos: %u, from:%u\n",
+                      packet->frame->dlc, packet->frame->pool_pos, *from);
 
                 ret = can_framespool_free(packet->frame);
                 if (ret != 0) {
@@ -500,7 +503,6 @@ static int frescan_hook_frame_recv (const struct can_chip_t *chip,
         return 0;
 };
 
-
 /**
  * frescan_hook_frame_sent - frame sent hook
  *
@@ -519,14 +521,14 @@ static int frescan_hook_frame_sent(const struct can_chip_t *chip)
 
         packet = the_networks[chip->minor].last_packet;
 
+        id = frescan_id_get_field(packet->frame->id,
+                                  FRESCAN_FIELD_FRAG_ID);
+
         DEBUG(FRESCAN_SENT_HOOK_ENABLE_DEBUG,
-              "frame sent, minor:%u flags:0x%X\n",
-              chip->minor, packet->flags);
+              "frame sent, minor:%u flags:0x%X frag_id:0x%X\n",
+              chip->minor, packet->flags, id);
 
         if (packet->flags & FRESCAN_SS) {
-                id = frescan_id_get_field(packet->frame->id,
-                                          FRESCAN_FIELD_FRAG_ID);
-
                 DEBUG(FRESCAN_SENT_HOOK_ENABLE_DEBUG,
                       "calling frame_sent + program repl for id:%u\n", id);
 
@@ -589,7 +591,7 @@ static int frescan_hook_frame_sent(const struct can_chip_t *chip)
                         return ret;
                 }
 
-                // TODO: signal semaphores, server consume budget...
+                // TODO: signal semaphore for send_sync
         }
 
         the_networks[chip->minor].last_packet = NULL;
@@ -602,3 +604,66 @@ static int frescan_hook_frame_sent(const struct can_chip_t *chip)
 
         return 0;
 };
+
+/**
+ * frescan_hook_frame_aborted - frame frame aborted hook
+ *
+ * This function will be called by the CAN driver's IRQ handler when a frame
+ * is aborted (because another frame with higher priority is waiting). We
+ * have to requeue the frame and update the buffer.
+ */
+
+static int frescan_hook_frame_aborted(const struct can_chip_t *chip)
+{
+        int ret;
+        frescan_packet_t *packet;
+        frescan_prio_queue_t *pqueue;
+        frescan_prio_t prio;
+        frescan_ss_t id;
+
+        packet = the_networks[chip->minor].last_packet;
+
+        id = frescan_id_get_field(packet->frame->id,
+                                  FRESCAN_FIELD_FRAG_ID);
+
+        DEBUG(FRESCAN_SENT_HOOK_ENABLE_DEBUG,
+              "frame aborted, minor:%u flags:0x%X frag_id:0x%X\n",
+              chip->minor, packet->flags, id);
+
+        if (packet->flags & FRESCAN_FP) {
+                prio = frescan_id_get_field(packet->frame->id,
+                                            FRESCAN_FIELD_PRIO);
+
+                DEBUG(FRESCAN_FRAG_ENABLE_DEBUG,
+                      "requeue fp packet, prio:%u\n", prio);
+
+                pqueue = the_networks[chip->minor].queues.tx_fp_queue;
+                ret = frescan_pqueue_requeue(pqueue, packet, prio);
+                if (ret != 0) {
+                        ERROR("could not requeue the packet\n");
+                        return -1;
+                }
+        } else if (packet->flags & FRESCAN_SS) {
+                DEBUG(FRESCAN_FRAG_ENABLE_DEBUG,
+                      "requeue server %u packet\n", id);
+
+                ret = frescan_servers_requeue(chip->minor, id, packet);
+                if (ret != 0) {
+                        ERROR("could not requeue the packet\n");
+                        return -1;
+                }
+        } else {
+                ERROR("flags are not correct\n");
+                return -1;
+        }
+
+        the_networks[chip->minor].last_packet = NULL;
+
+        ret = frescan_hw_buffer_update(chip->minor);
+        if (ret != 0) {
+                ERROR("could not update hw buffer\n");
+                return -1;
+        }
+
+        return 0;
+}