src_frescan/frescan.c

   1 /*!
   2  * @file frescan.c
   3  *
   4  * @brief the FRESCAN protocol
   5  *
   6  * @version 0.01
   7  *
   8  * @date 20-Feb-2008
   9  *
  10  * @author
  11  *      Daniel Sangorrin
  12  *
  13  * @comments
  14  *
  15  * This file contains the implementation of the FRESCAN protocol
  16  *
  17  * @license
  18  *
  19  * See MaRTE OS license
  20  *
  21  */
  22
  23 #include <sys/marte_configuration_parameters.h> // PATH_MX
  24 #include <fcntl.h>  // open
  25 #include <unistd.h> // ioctl
  26 #include <stdlib.h> // malloc
  27 #include <string.h> // memcpy
  28
  29 #include <drivers/can.h>       // can_chip_t, can_frame_t
  30 #include "frescan.h"           // frescan_init_params_t, frescan_send_params_t
  31 #include "frescan_queues.h"    // init, enqueue, requeue
  32 #include "frescan_data.h"      // init, the_networks
  33 #include "frescan_servers.h"   // init, frame_sent
  34 #include "frescan_debug.h"     // DEBUG
  35 #include "frescan_id.h"        // frescan_id_set_field, frescan_id_get_field
  36 #include "frescan_hw_buffer.h" // frescan_hw_buffer_update
  37 #include "frescan_reply_objects.h"     // frescan_replyobjects_init
  38 #include "frescan_servers_replenishments.h" // frescan_replenishments_xxx
  39 #include "frescan_packets.h"
  40
  41 static int frescan_hook_frame_recv (const struct can_chip_t *chip,
  42                                     struct can_frame_t *frame);
  43
  44 static int frescan_hook_frame_sent(const struct can_chip_t *chip);
  45
  46 /**
  47  * frescan_init - initializes the network and the internal structures
  48  *
  49  * @params: the initialization parameters
  50  *
  51  * This function initializes the frescan network protocol.
  52  *
  53  * First it opens and configures the corresponding CAN chip device. For the
  54  * CAN chip acceptance filter we use a dual filter configuration. The first
  55  * filter is to set my local address address and the second one is to allow
  56  * broadcast messages.
  57  *
  58  * Once the CAN chip is configured we call the initialization functions of
  59  * the rest of modules of frescan.
  60  *
  61  */
  62
  63 int frescan_init(frescan_init_params_t *params)
  64 {
  65         int fd, ret;
  66         char can_path[PATH_MX];
  67         struct ioctl_filters_t ioctl_filters;
  68         struct can_filter_t filters[2];
  69
  70         snprintf(can_path, PATH_MX, "/dev/can%u", params->net);
  71
  72         DEBUG(FRESCAN_INIT_ENABLE_DEBUG, "open %s\n", can_path);
  73
  74         fd = open (can_path, O_RDWR);
  75         if (fd == -1) {
  76                 ERROR ("could not open /dev/can%u\n", params->net);
  77                 return -1;
  78         }
  79
  80         DEBUG(FRESCAN_INIT_ENABLE_DEBUG, "set acceptance filters\n");
  81
  82         filters[0].mask = 0xFFFFFFFF;
  83         frescan_id_set_field(&filters[0].mask, FRESCAN_FIELD_DEST,0x00);
  84         filters[0].code = 0;
  85         frescan_id_set_field(&filters[0].code, FRESCAN_FIELD_DEST,params->node);
  86
  87         filters[1].mask = filters[0].mask;
  88         filters[1].code = 0;
  89         frescan_id_set_field(&filters[1].code,
  90                              FRESCAN_FIELD_DEST,
  91                              FRESCAN_BROADCAST_ADDR);
  92
  93         ioctl_filters.filters = filters;
  94         ioctl_filters.len = 2;
  95
  96         ret = ioctl(fd, CAN_IOCTL_SET_FILTERS, &ioctl_filters);
  97         if (ret == -1) {
  98                 ERROR ("ioctl CAN_IOCTL_SET_FILTERS failed /dev/can%u\n",
  99                        params->net);
 100                 return -1;
 101         }
 102
 103         DEBUG(FRESCAN_INIT_ENABLE_DEBUG, "set tx and rx hooks\n");
 104
 105         ret = ioctl(fd, CAN_IOCTL_SET_TX_HOOK, frescan_hook_frame_sent);
 106         if (ret == -1) {
 107                 ERROR ("ioctl CAN_IOCTL_SET_TX_HOOK failed /dev/can%u\n",
 108                        params->net);
 109                 return -1;
 110         }
 111
 112         ret = ioctl(fd, CAN_IOCTL_SET_RX_HOOK, frescan_hook_frame_recv);
 113         if (ret == -1) {
 114                 ERROR ("ioctl CAN_IOCTL_SET_RX_HOOK failed /dev/can%u\n",
 115                        params->net);
 116                 return -1;
 117         }
 118
 119         DEBUG(FRESCAN_INIT_ENABLE_DEBUG, "init the rest of modules\n");
 120
 121         ret = frescan_data_init(fd, params);
 122         if (ret != 0) {
 123                 ERROR("could not initialize the global data\n");
 124                 return -1;
 125         }
 126
 127         ret = frescan_packets_init();
 128         if (ret != 0) {
 129                 ERROR("could not initialize the packets pool\n");
 130                 return -1;
 131         }
 132
 133         ret = frescan_queues_init(&the_networks[params->net].queues, params);
 134         if (ret != 0) {
 135                 ERROR("could not initialize the queues\n");
 136                 return -1;
 137         }
 138
 139         ret = frescan_servers_init(params->net);
 140         if (ret != 0) {
 141                 ERROR("could not initialize the servers\n");
 142                 return -1;
 143         }
 144
 145         ret = frescan_replenishments_init(params->net);
 146         if (ret != 0) {
 147                 ERROR("could not initialize the replenishments\n");
 148                 return -1;
 149         }
 150
 151         ret = frescan_replyobjects_init(FRESCAN_REPLY_OBJECTS_MX_CEILING);
 152         if (ret != 0) {
 153                 ERROR("could not initialize the reply objects\n");
 154                 return -1;
 155         }
 156
 157         return 0;
 158 }
 159
 160 /**
 161  * frescan_send - send a message
 162  *
 163  * @params: the parameters needed by the protocol to send the message
 164  * @msg: the message buffer
 165  * @size: the size of the message
 166  *
 167  * This is one of the main functions of the protocol and it provides the
 168  * means to send a message through the protocol stack.
 169  */
 170
 171 int frescan_send(const frescan_send_params_t *params,
 172                  const uint8_t *msg,
 173                  const size_t size)
 174 {
 175         int ret;
 176         frescan_packet_t *packet;
 177         frescan_prio_queue_t *pqueue;
 178
 179         DEBUG(FRESCAN_SEND_ENABLE_DEBUG || FRESCAN_FRAG_ENABLE_DEBUG,
 180               "checking arguments (msg size=%d)\n", size);
 181
 182         if ((params == NULL) || (msg == NULL) || (size == 0)) {
 183                 ERROR("arguments are not ok\n");
 184                 return -1;
 185         }
 186
 187         DEBUG(FRESCAN_SEND_ENABLE_DEBUG, "allocating a packet\n");
 188
 189         FRESCAN_ACQUIRE_LOCK(&the_networks[params->net].lock);
 190         packet = frescan_packets_alloc();
 191         FRESCAN_RELEASE_LOCK(&the_networks[params->net].lock);
 192
 193         if (packet == NULL) {
 194                 ERROR("could not allocate packet\n");
 195                 return -1;
 196         }
 197         packet->flags = params->flags; // set the flags (to remember them)
 198
 199         DEBUG(FRESCAN_SEND_ENABLE_DEBUG, "allocating a frame\n");
 200
 201         FRESCAN_ACQUIRE_LOCK(&the_networks[params->net].lock);
 202         packet->frame = can_framespool_alloc();
 203         FRESCAN_RELEASE_LOCK(&the_networks[params->net].lock);
 204
 205         if (packet->frame == NULL) {
 206                 ERROR("could not allocate frame\n");
 207                 return -1;
 208         }
 209
 210         DEBUG(FRESCAN_SEND_ENABLE_DEBUG, "set values for the frame header\n");
 211         packet->frame->is_extended_format = true;
 212         packet->frame->is_rtr = false;
 213
 214         if (params->flags & FRESCAN_FP) {
 215                 // NOTE: frag id for fp is: FRESCAN_MX_IDS, so the servers can
 216                 // have IDs in the range (0 .. FRESCAN_MX_IDS-1)
 217                 frescan_id_set_field(&packet->frame->id,
 218                                      FRESCAN_FIELD_FRAG_ID,
 219                                      (uint32_t)FRESCAN_MX_IDS);
 220
 221                 frescan_id_set_field(&packet->frame->id,
 222                                       FRESCAN_FIELD_PRIO,
 223                                       (uint32_t)params->prio);
 224         } else {
 225                 // NOTE: the priority is put when the packet is dequeued
 226                 // and it is the priority of th server
 227                 frescan_id_set_field(&packet->frame->id,
 228                                      FRESCAN_FIELD_FRAG_ID,
 229                                      (uint32_t)params->ss);
 230         }
 231
 232         frescan_id_set_field(&packet->frame->id,
 233                              FRESCAN_FIELD_DEST,
 234                              (uint32_t)params->to);
 235
 236         frescan_id_set_field(&packet->frame->id,
 237                              FRESCAN_FIELD_SRC,
 238                              (uint32_t)the_networks[params->net].local_node);
 239
 240         frescan_id_set_field(&packet->frame->id,
 241                               FRESCAN_FIELD_CHAN,
 242                               (uint32_t)params->channel);
 243
 244         DEBUG(FRESCAN_SEND_ENABLE_DEBUG, "set the packet data bytes\n");
 245         if (params->flags & FRESCAN_ASYNC) {
 246                 // allocate a buffer and copy the data
 247                 // NOTE: instead of this we could use a chain of frames but
 248                 // i think it would be inefficient since each one can only
 249                 // hold 8 user bytes and we need to write its headers.
 250                 packet->buffer_head = (uint8_t *)malloc(size*sizeof(uint8_t));
 251                 memcpy(packet->buffer_head, msg, size);
 252         } else {
 253                 packet->buffer_head = (uint8_t *)msg;
 254         }
 255
 256         packet->buffer_read_pointer = packet->buffer_head;
 257         packet->buffer_pending_bytes = size;
 258         pqueue = the_networks[params->net].queues.tx_fp_queue;
 259
 260         DEBUG(FRESCAN_SEND_ENABLE_DEBUG, "enqueue the packet\n");
 261         if (packet->flags & FRESCAN_FP) {
 262                 FRESCAN_ACQUIRE_LOCK(&the_networks[params->net].lock);
 263                 ret = frescan_pqueue_enqueue(pqueue, packet, params->prio);
 264                 FRESCAN_RELEASE_LOCK(&the_networks[params->net].lock);
 265
 266                 if (ret != 0) {
 267                         ERROR("could not enqueue the packet\n");
 268                         return -1;
 269                 }
 270         } else {
 271                 FRESCAN_ACQUIRE_LOCK(&the_networks[params->net].lock);
 272                 ret = frescan_servers_enqueue(params->net, params->ss, packet);
 273                 FRESCAN_RELEASE_LOCK(&the_networks[params->net].lock);
 274
 275                 if (ret != 0) {
 276                         ERROR("could not enqueue the packet\n");
 277                         return -1;
 278                 }
 279         }
 280
 281         FRESCAN_ACQUIRE_LOCK(&the_networks[params->net].lock);
 282         ret = frescan_hw_buffer_update(params->net);
 283         FRESCAN_RELEASE_LOCK(&the_networks[params->net].lock);
 284
 285         if (ret != 0) {
 286                 ERROR("could not update hw buffer\n");
 287                 return -1;
 288         }
 289
 290         return 0;
 291 }
 292
 293 /**
 294  * frescan_recv - receive a message
 295  *
 296  * @params: the parameters needed by the protocol to receive the message
 297  * @msg: the message buffer
 298  * @size: the size of the message buffer
 299  * @recv_bytes: the number of bytes received
 300  * @from: the node that sent the message
 301  * @prio: the priority of the message
 302  *
 303  * This is one of the main functions of the protocol and it provides the
 304  * means to receive a message through the protocol stack.
 305  */
 306
 307 int frescan_recv(const frescan_recv_params_t *params,
 308                  uint8_t *msg,
 309                  const size_t size,
 310                  size_t *recv_bytes,
 311                  frescan_node_t *from,
 312                  frescan_prio_t *prio)
 313 {
 314         int ret;
 315         frescan_prio_queue_t *pqueue;
 316         bool blocking;
 317         frescan_packet_t *head, *packet;
 318
 319         if (params->flags & FRESCAN_SYNC) {
 320                 DEBUG(FRESCAN_RECV_ENABLE_DEBUG,
 321                       "receive a packet in blocking mode\n");
 322                 blocking = true;
 323         } else {
 324                 DEBUG(FRESCAN_RECV_ENABLE_DEBUG,
 325                       "receive a packet in non-blocking mode\n");
 326                 blocking = false;
 327         }
 328
 329         pqueue = the_networks[params->net].queues.rx_channel_queues
 330                                                              [params->channel];
 331
 332         FRESCAN_ACQUIRE_LOCK(&the_networks[params->net].lock);
 333         ret = frescan_pqueue_dequeue(pqueue, &head, prio, blocking);
 334         FRESCAN_RELEASE_LOCK(&the_networks[params->net].lock);
 335
 336         if (ret != 0) {
 337                 ERROR ("could not dequeue packet\n");
 338                 return -1;
 339         }
 340
 341         if (head == NULL) {
 342                 if (blocking == false) {
 343                         DEBUG(FRESCAN_RECV_ENABLE_DEBUG,
 344                               "blocking false, no packets\n");
 345                         *recv_bytes = 0;
 346                         return 0;
 347                 } else {
 348                         ERROR ("blocking true, and packet = null\n");
 349                         return -1;
 350                 }
 351         }
 352
 353         DEBUG(FRESCAN_RECV_ENABLE_DEBUG,
 354               "traverse the list of packets for this message\n");
 355
 356         *recv_bytes = 0;
 357
 358         FRESCAN_ACQUIRE_LOCK(&the_networks[params->net].lock);
 359
 360         list_for_each_entry(packet, &head->msg_list, msg_list) {
 361                 // TODO: memory checks, delete the packets
 362                 memcpy(msg + *recv_bytes,
 363                        packet->frame->data,
 364                        packet->frame->dlc);
 365                 *recv_bytes += packet->frame->dlc;
 366
 367                 *from = (frescan_node_t)frescan_id_get_field(packet->frame->id,
 368                                                              FRESCAN_FIELD_SRC);
 369                 DEBUG(FRESCAN_RECV_ENABLE_DEBUG,
 370                       "ID Packet, dlc: %u, frame pool pos: %u, from:%u\n",
 371                       packet->frame->dlc, packet->frame->pool_pos, *from);
 372
 373                 ret = can_framespool_free(packet->frame);
 374                 if (ret != 0) {
 375                         ERROR("could not free frame\n");
 376                         return -1;
 377                 }
 378
 379                 ret = frescan_packets_free(packet);
 380                 if (ret != 0) {
 381                         ERROR("could not free packet\n");
 382                         return -1;
 383                 }
 384         }
 385
 386         ret = frescan_packets_free(head);
 387
 388         FRESCAN_RELEASE_LOCK(&the_networks[params->net].lock);
 389
 390         if (ret != 0) {
 391                 ERROR("could not free head packet\n");
 392                 return -1;
 393         }
 394
 395         DEBUG(FRESCAN_RECV_ENABLE_DEBUG,
 396               "received bytes: %u\n", *recv_bytes);
 397
 398         return 0;
 399 }
 400
 401
 402 /**
 403  * frescan_hook_frame_recv - frame received hook
 404  *
 405  * This function will be called by the CAN driver's IRQ handler when a frame
 406  * is received so we can store it in an appropiate queue.
 407  *
 408  * NOTE: in the future it could consist simply of signaling a semaphore to
 409  * let a bottom half thread do the hard work.
 410  */
 411
 412
 413 static int frescan_hook_frame_recv (const struct can_chip_t *chip,
 414                                     struct can_frame_t *frame)
 415 {
 416         int i, ret;
 417         uint32_t prio, dest, src, channel, frag_id, frag_flag;
 418         frescan_packet_t *packet, *head;
 419         frescan_prio_queue_t *pqueue;
 420         int net;
 421
 422         net = chip->minor;
 423
 424         DEBUG(FRESCAN_RX_HOOK_ENABLE_DEBUG, "received a frame, net=%d\n", net);
 425         DEBUG(FRESCAN_RX_HOOK_ENABLE_DEBUG || FRESCAN_FRAG_ENABLE_DEBUG,
 426               "%s %s, id=0x%X, dlc=%u, pool:%u\n",
 427               (frame->is_extended_format) ? "Ext" : "Stnd",
 428               (frame->is_rtr) ? "RTR Frame" : "DATA Frame",
 429               frame->id,
 430               frame->dlc,
 431               frame->pool_pos);
 432
 433         for (i=0; i<frame->dlc; i++) {
 434                 DEBUG(FRESCAN_RX_HOOK_ENABLE_DEBUG,
 435                       "data[%d] = 0x%X;\n", i, frame->data[i]);
 436         }
 437
 438         prio      = frescan_id_get_field(frame->id, FRESCAN_FIELD_PRIO);
 439         dest      = frescan_id_get_field(frame->id, FRESCAN_FIELD_DEST);
 440         src       = frescan_id_get_field(frame->id, FRESCAN_FIELD_SRC);
 441         channel   = frescan_id_get_field(frame->id, FRESCAN_FIELD_CHAN);
 442         frag_id   = frescan_id_get_field(frame->id, FRESCAN_FIELD_FRAG_ID);
 443         frag_flag = frescan_id_get_field(frame->id, FRESCAN_FIELD_FRAG_FLAG);
 444
 445         DEBUG(FRESCAN_RX_HOOK_ENABLE_DEBUG || FRESCAN_FRAG_ENABLE_DEBUG,
 446               "prio:%u dest:%u src:%u chan:%u id:%u flag:%u\n",
 447               prio, dest, src, channel, frag_id, frag_flag);
 448
 449         DEBUG(FRESCAN_RX_HOOK_ENABLE_DEBUG,
 450               "enqueue the packet in ID queue\n");
 451         packet = frescan_packets_alloc();
 452         packet->frame = frame;
 453
 454         if (frag_id == 0) {
 455                 head = the_networks[net].id_fp_queues[prio];
 456         } else {
 457                 head = the_networks[net].id_queues[frag_id];
 458         }
 459
 460         if (head == NULL) {
 461                 DEBUG(FRESCAN_RX_HOOK_ENABLE_DEBUG ||
 462                                 FRESCAN_FRAG_ENABLE_DEBUG,
 463                 "allocate head for id=%u\n", frag_id);
 464                 head = frescan_packets_alloc();
 465                 INIT_LIST_HEAD(&head->msg_list);
 466
 467                 if (frag_id == 0) {
 468                         the_networks[net].id_fp_queues[prio] = head;
 469                 } else {
 470                         the_networks[net].id_queues[frag_id] = head;
 471                 }
 472         }
 473
 474         list_add_tail(&packet->msg_list, &head->msg_list);
 475
 476         if (frag_flag == false) {
 477                 DEBUG(FRESCAN_RX_HOOK_ENABLE_DEBUG ||
 478                                 FRESCAN_FRAG_ENABLE_DEBUG,
 479                 "message complete, move msg to channel\n");
 480                 // TODO: select the highest priority??
 481                 pqueue = the_networks[net].queues.rx_channel_queues[channel];
 482                 ret = frescan_pqueue_enqueue(pqueue, head, prio);
 483                 if (ret != 0) {
 484                         ERROR("could not enqueue message in channel queue\n");
 485                         return -1;
 486                 }
 487
 488                 if (frag_id == 0) {
 489                         the_networks[net].id_fp_queues[prio] = NULL;
 490                 } else {
 491                         the_networks[net].id_queues[frag_id] = NULL;
 492                 }
 493
 494         } else {
 495                 DEBUG(FRESCAN_RX_HOOK_ENABLE_DEBUG ||
 496                                 FRESCAN_FRAG_ENABLE_DEBUG,
 497                 "message not complete, wait for more fragments\n");
 498         }
 499
 500         // NOTE: don't forget to free the frame and the packet when it is
 501         // read by the user application
 502
 503         return 0;
 504 };
 505
 506
 507 /**
 508  * frescan_hook_frame_sent - frame sent hook
 509  *
 510  * This function will be called by the CAN driver's IRQ handler when a frame
 511  * is sent through the CAN bus so we can enqueue another one, signal a
 512  * semaphore, consume sporadic server capacity...
 513  */
 514
 515 static int frescan_hook_frame_sent(const struct can_chip_t *chip)
 516 {
 517         int ret;
 518         frescan_packet_t *packet;
 519         frescan_prio_queue_t *pqueue;
 520         frescan_prio_t prio;
 521         frescan_ss_t id;
 522
 523         packet = the_networks[chip->minor].last_packet;
 524
 525         DEBUG(FRESCAN_SENT_HOOK_ENABLE_DEBUG,
 526               "frame sent, minor:%u flags:0x%X\n",
 527               chip->minor, packet->flags);
 528
 529         if (packet->flags & FRESCAN_SS) {
 530                 id = frescan_id_get_field(packet->frame->id,
 531                                           FRESCAN_FIELD_FRAG_ID);
 532
 533                 DEBUG(FRESCAN_SENT_HOOK_ENABLE_DEBUG,
 534                       "calling frame_sent + program repl for id:%u\n", id);
 535
 536                 ret = frescan_replenishment_program(chip->minor, id);
 537                 if (ret != 0) {
 538                         ERROR("could not program replenishment\n");
 539                         return -1;
 540                 }
 541
 542                 ret = frescan_servers_frame_sent(chip->minor, id);
 543                 if (ret != 0) {
 544                         ERROR("could not let the server a frame was sent\n");
 545                         return -1;
 546                 }
 547         }
 548
 549         DEBUG(FRESCAN_SENT_HOOK_ENABLE_DEBUG || FRESCAN_FRAG_ENABLE_DEBUG,
 550               "last packet buffer_pending_bytes=%u\n",
 551               packet->buffer_pending_bytes);
 552
 553         if (packet->buffer_pending_bytes > 0) {
 554                 if (packet->flags & FRESCAN_FP) {
 555                         prio = frescan_id_get_field(packet->frame->id,
 556                                         FRESCAN_FIELD_PRIO);
 557
 558                         DEBUG(FRESCAN_FRAG_ENABLE_DEBUG,
 559                               "requeue fp packet, prio:%u\n", prio);
 560
 561                         pqueue = the_networks[chip->minor].queues.tx_fp_queue;
 562                         ret = frescan_pqueue_requeue(pqueue, packet, prio);
 563                         if (ret != 0) {
 564                                 ERROR("could not requeue the packet\n");
 565                                 return -1;
 566                         }
 567                 } else if (packet->flags & FRESCAN_SS) {
 568                         DEBUG(FRESCAN_FRAG_ENABLE_DEBUG,
 569                               "requeue server %u packet\n", id);
 570                         ret = frescan_servers_requeue(chip->minor, id, packet);
 571                         if (ret != 0) {
 572                                 ERROR("could not requeue the packet\n");
 573                                 return -1;
 574                         }
 575                 } else {
 576                         ERROR("flags are not correct\n");
 577                         return -1;
 578                 }
 579         } else {
 580                 DEBUG(FRESCAN_FRAG_ENABLE_DEBUG,
 581                       "all packet fragmets sent, freeing the packet\n");
 582
 583                 ret = can_framespool_free(packet->frame);
 584                 if (ret != 0)  {
 585                         ERROR ("could not free the frame\n");
 586                         return ret;
 587                 }
 588
 589                 ret = frescan_packets_free(packet);
 590                 if (ret != 0)  {
 591                         ERROR ("could not free the packet\n");
 592                         return ret;
 593                 }
 594
 595                 // TODO: signal semaphores, server consume budget...
 596         }
 597
 598         the_networks[chip->minor].last_packet = NULL;
 599
 600         ret = frescan_hw_buffer_update(chip->minor);
 601         if (ret != 0) {
 602                 ERROR("could not update hw buffer\n");
 603                 return -1;
 604         }
 605
 606         return 0;
 607 };