X-Git-Url: https://rtime.felk.cvut.cz/gitweb/frescor/fna.git/blobdiff_plain/b8916a2947d44f3a6a97360657a482cd97705d40..5ac29d6d0804a77ef8f795b2006723b6b71d3566:/src_frescan/frescan.c diff --git a/src_frescan/frescan.c b/src_frescan/frescan.c index 8f1b9c4..4084c8c 100644 --- 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; +}