X-Git-Url: https://rtime.felk.cvut.cz/gitweb/frescor/fna.git/blobdiff_plain/97064f0217b0a4b4ab569834870321c497e5905d..45f5e058d668a5e8db88f743f14ebd77b91f02d8:/src_frescan/frescan.c diff --git a/src_frescan/frescan.c b/src_frescan/frescan.c index 0630342..c9b142a 100644 --- a/src_frescan/frescan.c +++ b/src_frescan/frescan.c @@ -16,7 +16,57 @@ * * @license * - * See MaRTE OS license +//---------------------------------------------------------------------- +// Copyright (C) 2006 - 2009 by the FRESCOR consortium: +// +// Universidad de Cantabria, SPAIN +// University of York, UK +// Scuola Superiore Sant'Anna, ITALY +// Kaiserslautern University, GERMANY +// Univ. Politecnica Valencia, SPAIN +// Czech Technical University in Prague, CZECH REPUBLIC +// ENEA SWEDEN +// Thales Communication S.A. FRANCE +// Visual Tools S.A. SPAIN +// Rapita Systems Ltd UK +// Evidence ITALY +// +// See http://www.frescor.org +// +// The FRESCOR project (FP6/2005/IST/5-034026) is funded +// in part by the European Union Sixth Framework Programme +// The European Union is not liable of any use that may be +// made of this code. +// +// +// based on previous work (FSF) done in the FIRST project +// +// Copyright (C) 2005 Mälardalen University, SWEDEN +// Scuola Superiore S.Anna, ITALY +// Universidad de Cantabria, SPAIN +// University of York, UK +// +// This file is part of FNA (Frescor Network Adaptation) +// +// FNA is free software; you can redistribute it and/or modify it +// under terms of the GNU General Public License as published by the +// Free Software Foundation; either version 2, or (at your option) any +// later version. FNA is distributed in the hope that it will be +// useful, but WITHOUT ANY WARRANTY; without even the implied warranty +// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. You should have received a +// copy of the GNU General Public License along with FNA; see file +// COPYING. If not, write to the Free Software Foundation, 675 Mass Ave, +// Cambridge, MA 02139, USA. +// +// As a special exception, including FNA header files in a file, +// instantiating FNA generics or templates, or linking other files +// with FNA objects to produce an executable application, does not +// by itself cause the resulting executable application to be covered +// by the GNU General Public License. This exception does not +// however invalidate any other reasons why the executable file might be +// covered by the GNU Public License. +// ----------------------------------------------------------------------- * */ @@ -25,17 +75,16 @@ #include // ioctl #include // malloc #include // memcpy +#include #include // can_chip_t, can_frame_t #include "frescan.h" // frescan_init_params_t, frescan_send_params_t #include "frescan_queues.h" // init, enqueue, requeue -#include "frescan_data.h" // init, the_networks +#include "frescan_data.h" // init, frescan_data #include "frescan_servers.h" // init, frame_sent #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_servers_replenishments.h" // frescan_replenishments_xxx #include "frescan_packets.h" static int frescan_hook_frame_recv (const struct can_chip_t *chip, @@ -43,6 +92,8 @@ static int frescan_hook_frame_recv (const struct can_chip_t *chip, 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 * @@ -73,7 +124,8 @@ int frescan_init(frescan_init_params_t *params) fd = open (can_path, O_RDWR); if (fd == -1) { - ERROR ("could not open /dev/can%u\n", params->net); + FRESCAN_ERROR ("could not open /dev/can%u\n", params->net); + FRESCAN_ERROR ("hint: check driver or card installation\n"); return -1; } @@ -95,23 +147,30 @@ int frescan_init(frescan_init_params_t *params) ret = ioctl(fd, CAN_IOCTL_SET_FILTERS, &ioctl_filters); if (ret == -1) { - ERROR ("ioctl CAN_IOCTL_SET_FILTERS failed /dev/can%u\n", + FRESCAN_ERROR ("ioctl CAN_IOCTL_SET_FILTERS failed /dev/can%u\n", params->net); 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) { - ERROR ("ioctl CAN_IOCTL_SET_TX_HOOK failed /dev/can%u\n", + FRESCAN_ERROR ("ioctl CAN_IOCTL_SET_TX_HOOK failed /dev/can%u\n", params->net); return -1; } ret = ioctl(fd, CAN_IOCTL_SET_RX_HOOK, frescan_hook_frame_recv); if (ret == -1) { - ERROR ("ioctl CAN_IOCTL_SET_RX_HOOK failed /dev/can%u\n", + FRESCAN_ERROR ("ioctl CAN_IOCTL_SET_RX_HOOK failed /dev/can%u\n", + params->net); + return -1; + } + + ret = ioctl(fd, CAN_IOCTL_SET_AB_HOOK, frescan_hook_frame_aborted); + if (ret == -1) { + FRESCAN_ERROR ("ioctl CAN_IOCTL_SET_AB_HOOK failed /dev/can%u\n", params->net); return -1; } @@ -120,37 +179,25 @@ int frescan_init(frescan_init_params_t *params) ret = frescan_data_init(fd, params); if (ret != 0) { - ERROR("could not initialize the global data\n"); + FRESCAN_ERROR("could not initialize the global data\n"); return -1; } ret = frescan_packets_init(); if (ret != 0) { - ERROR("could not initialize the packets pool\n"); + FRESCAN_ERROR("could not initialize the packets pool\n"); return -1; } - ret = frescan_queues_init(&the_networks[params->net].queues, params); + ret = frescan_queues_init(&frescan_data[params->net].queues, params); if (ret != 0) { - ERROR("could not initialize the queues\n"); + FRESCAN_ERROR("could not initialize the queues\n"); return -1; } ret = frescan_servers_init(params->net); if (ret != 0) { - ERROR("could not initialize the servers\n"); - return -1; - } - - ret = frescan_replenishments_init(params->net); - if (ret != 0) { - ERROR("could not initialize the replenishments\n"); - return -1; - } - - ret = frescan_replyobjects_init(FRESCAN_REPLY_OBJECTS_MX_CEILING); - if (ret != 0) { - ERROR("could not initialize the reply objects\n"); + FRESCAN_ERROR("could not initialize the servers\n"); return -1; } @@ -180,30 +227,30 @@ int frescan_send(const frescan_send_params_t *params, "checking arguments (msg size=%d)\n", size); if ((params == NULL) || (msg == NULL) || (size == 0)) { - ERROR("arguments are not ok\n"); + FRESCAN_ERROR("arguments are not ok\n"); return -1; } DEBUG(FRESCAN_SEND_ENABLE_DEBUG, "allocating a packet\n"); - FRESCAN_ACQUIRE_LOCK(&the_networks[params->net].lock); + FRESCAN_ACQUIRE_LOCK(&frescan_data[params->net].lock); packet = frescan_packets_alloc(); - FRESCAN_RELEASE_LOCK(&the_networks[params->net].lock); + FRESCAN_RELEASE_LOCK(&frescan_data[params->net].lock); if (packet == NULL) { - ERROR("could not allocate packet\n"); + FRESCAN_ERROR("could not allocate packet\n"); return -1; } packet->flags = params->flags; // set the flags (to remember them) DEBUG(FRESCAN_SEND_ENABLE_DEBUG, "allocating a frame\n"); - FRESCAN_ACQUIRE_LOCK(&the_networks[params->net].lock); + FRESCAN_ACQUIRE_LOCK(&frescan_data[params->net].lock); packet->frame = can_framespool_alloc(); - FRESCAN_RELEASE_LOCK(&the_networks[params->net].lock); + FRESCAN_RELEASE_LOCK(&frescan_data[params->net].lock); if (packet->frame == NULL) { - ERROR("could not allocate frame\n"); + FRESCAN_ERROR("could not allocate frame\n"); return -1; } @@ -235,7 +282,7 @@ int frescan_send(const frescan_send_params_t *params, frescan_id_set_field(&packet->frame->id, FRESCAN_FIELD_SRC, - (uint32_t)the_networks[params->net].local_node); + (uint32_t)frescan_data[params->net].local_node); frescan_id_set_field(&packet->frame->id, FRESCAN_FIELD_CHAN, @@ -247,7 +294,9 @@ int frescan_send(const frescan_send_params_t *params, // NOTE: instead of this we could use a chain of frames but // i think it would be inefficient since each one can only // hold 8 user bytes and we need to write its headers. - packet->buffer_head = (uint8_t *)malloc(size*sizeof(uint8_t)); + FRESCAN_ACQUIRE_LOCK(&frescan_data[params->net].lock); + packet->buffer_head = (uint8_t *)malloc(size*sizeof(uint8_t)); // TODO: FREE IT!!! + FRESCAN_RELEASE_LOCK(&frescan_data[params->net].lock); memcpy(packet->buffer_head, msg, size); } else { packet->buffer_head = (uint8_t *)msg; @@ -255,35 +304,35 @@ int frescan_send(const frescan_send_params_t *params, packet->buffer_read_pointer = packet->buffer_head; packet->buffer_pending_bytes = size; - pqueue = the_networks[params->net].queues.tx_fp_queue; + pqueue = frescan_data[params->net].queues.tx_fp_queue; DEBUG(FRESCAN_SEND_ENABLE_DEBUG, "enqueue the packet\n"); if (packet->flags & FRESCAN_FP) { - FRESCAN_ACQUIRE_LOCK(&the_networks[params->net].lock); + FRESCAN_ACQUIRE_LOCK(&frescan_data[params->net].lock); ret = frescan_pqueue_enqueue(pqueue, packet, params->prio); - FRESCAN_RELEASE_LOCK(&the_networks[params->net].lock); + FRESCAN_RELEASE_LOCK(&frescan_data[params->net].lock); if (ret != 0) { - ERROR("could not enqueue the packet\n"); + FRESCAN_ERROR("could not enqueue the packet\n"); return -1; } } else { - FRESCAN_ACQUIRE_LOCK(&the_networks[params->net].lock); + FRESCAN_ACQUIRE_LOCK(&frescan_data[params->net].lock); ret = frescan_servers_enqueue(params->net, params->ss, packet); - FRESCAN_RELEASE_LOCK(&the_networks[params->net].lock); + FRESCAN_RELEASE_LOCK(&frescan_data[params->net].lock); if (ret != 0) { - ERROR("could not enqueue the packet\n"); + FRESCAN_ERROR("could not enqueue the packet\n"); return -1; } } - FRESCAN_ACQUIRE_LOCK(&the_networks[params->net].lock); + FRESCAN_ACQUIRE_LOCK(&frescan_data[params->net].lock); ret = frescan_hw_buffer_update(params->net); - FRESCAN_RELEASE_LOCK(&the_networks[params->net].lock); + FRESCAN_RELEASE_LOCK(&frescan_data[params->net].lock); if (ret != 0) { - ERROR("could not update hw buffer\n"); + FRESCAN_ERROR("could not update hw buffer\n"); return -1; } @@ -326,15 +375,12 @@ int frescan_recv(const frescan_recv_params_t *params, blocking = false; } - pqueue = the_networks[params->net].queues.rx_channel_queues + pqueue = frescan_data[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"); + FRESCAN_ERROR ("could not dequeue packet\n"); return -1; } @@ -345,7 +391,7 @@ int frescan_recv(const frescan_recv_params_t *params, *recv_bytes = 0; return 0; } else { - ERROR ("blocking true, and packet = null\n"); + FRESCAN_ERROR ("blocking true, and packet = null\n"); return -1; } } @@ -355,7 +401,7 @@ int frescan_recv(const frescan_recv_params_t *params, *recv_bytes = 0; - FRESCAN_ACQUIRE_LOCK(&the_networks[params->net].lock); + FRESCAN_ACQUIRE_LOCK(&frescan_data[params->net].lock); list_for_each_entry(packet, &head->msg_list, msg_list) { // TODO: memory checks, delete the packets @@ -372,23 +418,23 @@ int frescan_recv(const frescan_recv_params_t *params, ret = can_framespool_free(packet->frame); if (ret != 0) { - ERROR("could not free frame\n"); + FRESCAN_ERROR("could not free frame\n"); return -1; } ret = frescan_packets_free(packet); if (ret != 0) { - ERROR("could not free packet\n"); + FRESCAN_ERROR("could not free packet\n"); return -1; } } ret = frescan_packets_free(head); - FRESCAN_RELEASE_LOCK(&the_networks[params->net].lock); + FRESCAN_RELEASE_LOCK(&frescan_data[params->net].lock); if (ret != 0) { - ERROR("could not free head packet\n"); + FRESCAN_ERROR("could not free head packet\n"); return -1; } @@ -445,16 +491,21 @@ static int frescan_hook_frame_recv (const struct can_chip_t *chip, DEBUG(FRESCAN_RX_HOOK_ENABLE_DEBUG || FRESCAN_FRAG_ENABLE_DEBUG, "prio:%u dest:%u src:%u chan:%u id:%u flag:%u\n", prio, dest, src, channel, frag_id, frag_flag); - DEBUG(FRESCAN_RX_HOOK_ENABLE_DEBUG, "enqueue the packet in ID queue\n"); + packet = frescan_packets_alloc(); + if (packet == NULL) { + FRESCAN_ERROR("could not allocate packet\n"); + return -1; + } + packet->frame = frame; - if (frag_id == 0) { - head = the_networks[net].id_fp_queues[prio]; + if (frag_id == FRESCAN_MX_IDS) { + head = frescan_data[net].id_fp_queues[src][prio]; } else { - head = the_networks[net].id_queues[frag_id]; + head = frescan_data[net].id_queues[src][frag_id]; } if (head == NULL) { @@ -462,12 +513,17 @@ static int frescan_hook_frame_recv (const struct can_chip_t *chip, FRESCAN_FRAG_ENABLE_DEBUG, "allocate head for id=%u\n", frag_id); head = frescan_packets_alloc(); + if (head == NULL) { + FRESCAN_ERROR("could not allocate packet\n"); + return -1; + } + INIT_LIST_HEAD(&head->msg_list); - if (frag_id == 0) { - the_networks[net].id_fp_queues[prio] = head; + if (frag_id == FRESCAN_MX_IDS) { + frescan_data[net].id_fp_queues[src][prio] = head; } else { - the_networks[net].id_queues[frag_id] = head; + frescan_data[net].id_queues[src][frag_id] = head; } } @@ -478,17 +534,17 @@ static int frescan_hook_frame_recv (const struct can_chip_t *chip, FRESCAN_FRAG_ENABLE_DEBUG, "message complete, move msg to channel\n"); // TODO: select the highest priority?? - pqueue = the_networks[net].queues.rx_channel_queues[channel]; + pqueue = frescan_data[net].queues.rx_channel_queues[channel]; ret = frescan_pqueue_enqueue(pqueue, head, prio); if (ret != 0) { - ERROR("could not enqueue message in channel queue\n"); + FRESCAN_ERROR("could not enqueue message in channel queue\n"); return -1; } - if (frag_id == 0) { - the_networks[net].id_fp_queues[prio] = NULL; + if (frag_id == FRESCAN_MX_IDS) { + frescan_data[net].id_fp_queues[src][prio] = NULL; } else { - the_networks[net].id_queues[frag_id] = NULL; + frescan_data[net].id_queues[src][frag_id] = NULL; } } else { @@ -503,7 +559,6 @@ static int frescan_hook_frame_recv (const struct can_chip_t *chip, return 0; }; - /** * frescan_hook_frame_sent - frame sent hook * @@ -520,28 +575,23 @@ static int frescan_hook_frame_sent(const struct can_chip_t *chip) frescan_prio_t prio; frescan_ss_t id; - packet = the_networks[chip->minor].last_packet; + packet = frescan_data[chip->minor].last_packet; + assert(packet != NULL); + + 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); - ret = frescan_replenishment_program(chip->minor, id); + ret = frescan_servers_frame_sent(chip->minor, id, packet); if (ret != 0) { - ERROR("could not program replenishment\n"); - return -1; - } - - ret = frescan_servers_frame_sent(chip->minor, id); - if (ret != 0) { - ERROR("could not let the server a frame was sent\n"); + FRESCAN_ERROR("could not let the server a frame was sent\n"); return -1; } } @@ -558,10 +608,10 @@ static int frescan_hook_frame_sent(const struct can_chip_t *chip) DEBUG(FRESCAN_FRAG_ENABLE_DEBUG, "requeue fp packet, prio:%u\n", prio); - pqueue = the_networks[chip->minor].queues.tx_fp_queue; + pqueue = frescan_data[chip->minor].queues.tx_fp_queue; ret = frescan_pqueue_requeue(pqueue, packet, prio); if (ret != 0) { - ERROR("could not requeue the packet\n"); + FRESCAN_ERROR("could not requeue the packet packet:%p flags:0x%X\n", packet, packet->flags); return -1; } } else if (packet->flags & FRESCAN_SS) { @@ -569,11 +619,11 @@ static int frescan_hook_frame_sent(const struct can_chip_t *chip) "requeue server %u packet\n", id); ret = frescan_servers_requeue(chip->minor, id, packet); if (ret != 0) { - ERROR("could not requeue the packet\n"); + FRESCAN_ERROR("could not requeue the packet\n"); return -1; } } else { - ERROR("flags are not correct\n"); + FRESCAN_ERROR("flags are not correct\n"); return -1; } } else { @@ -582,26 +632,89 @@ static int frescan_hook_frame_sent(const struct can_chip_t *chip) ret = can_framespool_free(packet->frame); if (ret != 0) { - ERROR ("could not free the frame\n"); + FRESCAN_ERROR ("could not free the frame\n"); return ret; } ret = frescan_packets_free(packet); if (ret != 0) { - ERROR ("could not free the packet\n"); + FRESCAN_ERROR ("could not free the packet\n"); return ret; } - // TODO: signal semaphores, server consume budget... + // TODO: signal semaphore for send_sync } - the_networks[chip->minor].last_packet = NULL; + frescan_data[chip->minor].last_packet = NULL; ret = frescan_hw_buffer_update(chip->minor); if (ret != 0) { - ERROR("could not update hw buffer\n"); + FRESCAN_ERROR("could not update hw buffer\n"); return -1; } 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 = frescan_data[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 = frescan_data[chip->minor].queues.tx_fp_queue; + ret = frescan_pqueue_requeue(pqueue, packet, prio); + if (ret != 0) { + FRESCAN_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) { + FRESCAN_ERROR("could not requeue the packet\n"); + return -1; + } + } else { + FRESCAN_ERROR("flags are not correct\n"); + return -1; + } + + frescan_data[chip->minor].last_packet = NULL; + + ret = frescan_hw_buffer_update(chip->minor); + if (ret != 0) { + FRESCAN_ERROR("could not update hw buffer\n"); + return -1; + } + + return 0; +}