4 * @brief the FRESCAN protocol
15 * This file contains the implementation of the FRESCAN protocol
19 * See MaRTE OS license
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
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"
41 static int frescan_hook_frame_recv (const struct can_chip_t *chip,
42 struct can_frame_t *frame);
44 static int frescan_hook_frame_sent(const struct can_chip_t *chip);
46 static int frescan_hook_frame_aborted(const struct can_chip_t *chip);
49 * frescan_init - initializes the network and the internal structures
51 * @params: the initialization parameters
53 * This function initializes the frescan network protocol.
55 * First it opens and configures the corresponding CAN chip device. For the
56 * CAN chip acceptance filter we use a dual filter configuration. The first
57 * filter is to set my local address address and the second one is to allow
60 * Once the CAN chip is configured we call the initialization functions of
61 * the rest of modules of frescan.
65 int frescan_init(frescan_init_params_t *params)
68 char can_path[PATH_MX];
69 struct ioctl_filters_t ioctl_filters;
70 struct can_filter_t filters[2];
72 snprintf(can_path, PATH_MX, "/dev/can%u", params->net);
74 DEBUG(FRESCAN_INIT_ENABLE_DEBUG, "open %s\n", can_path);
76 fd = open (can_path, O_RDWR);
78 ERROR ("could not open /dev/can%u\n", params->net);
82 DEBUG(FRESCAN_INIT_ENABLE_DEBUG, "set acceptance filters\n");
84 filters[0].mask = 0xFFFFFFFF;
85 frescan_id_set_field(&filters[0].mask, FRESCAN_FIELD_DEST,0x00);
87 frescan_id_set_field(&filters[0].code, FRESCAN_FIELD_DEST,params->node);
89 filters[1].mask = filters[0].mask;
91 frescan_id_set_field(&filters[1].code,
93 FRESCAN_BROADCAST_ADDR);
95 ioctl_filters.filters = filters;
96 ioctl_filters.len = 2;
98 ret = ioctl(fd, CAN_IOCTL_SET_FILTERS, &ioctl_filters);
100 ERROR ("ioctl CAN_IOCTL_SET_FILTERS failed /dev/can%u\n",
105 DEBUG(FRESCAN_INIT_ENABLE_DEBUG, "set tx, rx, abort hooks\n");
107 ret = ioctl(fd, CAN_IOCTL_SET_TX_HOOK, frescan_hook_frame_sent);
109 ERROR ("ioctl CAN_IOCTL_SET_TX_HOOK failed /dev/can%u\n",
114 ret = ioctl(fd, CAN_IOCTL_SET_RX_HOOK, frescan_hook_frame_recv);
116 ERROR ("ioctl CAN_IOCTL_SET_RX_HOOK failed /dev/can%u\n",
121 ret = ioctl(fd, CAN_IOCTL_SET_AB_HOOK, frescan_hook_frame_aborted);
123 ERROR ("ioctl CAN_IOCTL_SET_AB_HOOK failed /dev/can%u\n",
128 DEBUG(FRESCAN_INIT_ENABLE_DEBUG, "init the rest of modules\n");
130 ret = frescan_data_init(fd, params);
132 ERROR("could not initialize the global data\n");
136 ret = frescan_packets_init();
138 ERROR("could not initialize the packets pool\n");
142 ret = frescan_queues_init(&the_networks[params->net].queues, params);
144 ERROR("could not initialize the queues\n");
148 ret = frescan_servers_init(params->net);
150 ERROR("could not initialize the servers\n");
154 ret = frescan_replenishments_init(params->net);
156 ERROR("could not initialize the replenishments\n");
160 ret = frescan_replyobjects_init(FRESCAN_REPLY_OBJECTS_MX_CEILING);
162 ERROR("could not initialize the reply objects\n");
170 * frescan_send - send a message
172 * @params: the parameters needed by the protocol to send the message
173 * @msg: the message buffer
174 * @size: the size of the message
176 * This is one of the main functions of the protocol and it provides the
177 * means to send a message through the protocol stack.
180 int frescan_send(const frescan_send_params_t *params,
185 frescan_packet_t *packet;
186 frescan_prio_queue_t *pqueue;
188 DEBUG(FRESCAN_SEND_ENABLE_DEBUG || FRESCAN_FRAG_ENABLE_DEBUG,
189 "checking arguments (msg size=%d)\n", size);
191 if ((params == NULL) || (msg == NULL) || (size == 0)) {
192 ERROR("arguments are not ok\n");
196 DEBUG(FRESCAN_SEND_ENABLE_DEBUG, "allocating a packet\n");
198 FRESCAN_ACQUIRE_LOCK(&the_networks[params->net].lock);
199 packet = frescan_packets_alloc();
200 FRESCAN_RELEASE_LOCK(&the_networks[params->net].lock);
202 if (packet == NULL) {
203 ERROR("could not allocate packet\n");
206 packet->flags = params->flags; // set the flags (to remember them)
208 DEBUG(FRESCAN_SEND_ENABLE_DEBUG, "allocating a frame\n");
210 FRESCAN_ACQUIRE_LOCK(&the_networks[params->net].lock);
211 packet->frame = can_framespool_alloc();
212 FRESCAN_RELEASE_LOCK(&the_networks[params->net].lock);
214 if (packet->frame == NULL) {
215 ERROR("could not allocate frame\n");
219 DEBUG(FRESCAN_SEND_ENABLE_DEBUG, "set values for the frame header\n");
220 packet->frame->is_extended_format = true;
221 packet->frame->is_rtr = false;
223 if (params->flags & FRESCAN_FP) {
224 // NOTE: frag id for fp is: FRESCAN_MX_IDS, so the servers can
225 // have IDs in the range (0 .. FRESCAN_MX_IDS-1)
226 frescan_id_set_field(&packet->frame->id,
227 FRESCAN_FIELD_FRAG_ID,
228 (uint32_t)FRESCAN_MX_IDS);
230 frescan_id_set_field(&packet->frame->id,
232 (uint32_t)params->prio);
234 // NOTE: the priority is put when the packet is dequeued
235 // and it is the priority of th server
236 frescan_id_set_field(&packet->frame->id,
237 FRESCAN_FIELD_FRAG_ID,
238 (uint32_t)params->ss);
241 frescan_id_set_field(&packet->frame->id,
243 (uint32_t)params->to);
245 frescan_id_set_field(&packet->frame->id,
247 (uint32_t)the_networks[params->net].local_node);
249 frescan_id_set_field(&packet->frame->id,
251 (uint32_t)params->channel);
253 DEBUG(FRESCAN_SEND_ENABLE_DEBUG, "set the packet data bytes\n");
254 if (params->flags & FRESCAN_ASYNC) {
255 // allocate a buffer and copy the data
256 // NOTE: instead of this we could use a chain of frames but
257 // i think it would be inefficient since each one can only
258 // hold 8 user bytes and we need to write its headers.
259 packet->buffer_head = (uint8_t *)malloc(size*sizeof(uint8_t));
260 memcpy(packet->buffer_head, msg, size);
262 packet->buffer_head = (uint8_t *)msg;
265 packet->buffer_read_pointer = packet->buffer_head;
266 packet->buffer_pending_bytes = size;
267 pqueue = the_networks[params->net].queues.tx_fp_queue;
269 DEBUG(FRESCAN_SEND_ENABLE_DEBUG, "enqueue the packet\n");
270 if (packet->flags & FRESCAN_FP) {
271 FRESCAN_ACQUIRE_LOCK(&the_networks[params->net].lock);
272 ret = frescan_pqueue_enqueue(pqueue, packet, params->prio);
273 FRESCAN_RELEASE_LOCK(&the_networks[params->net].lock);
276 ERROR("could not enqueue the packet\n");
280 FRESCAN_ACQUIRE_LOCK(&the_networks[params->net].lock);
281 ret = frescan_servers_enqueue(params->net, params->ss, packet);
282 FRESCAN_RELEASE_LOCK(&the_networks[params->net].lock);
285 ERROR("could not enqueue the packet\n");
290 FRESCAN_ACQUIRE_LOCK(&the_networks[params->net].lock);
291 ret = frescan_hw_buffer_update(params->net);
292 FRESCAN_RELEASE_LOCK(&the_networks[params->net].lock);
295 ERROR("could not update hw buffer\n");
303 * frescan_recv - receive a message
305 * @params: the parameters needed by the protocol to receive the message
306 * @msg: the message buffer
307 * @size: the size of the message buffer
308 * @recv_bytes: the number of bytes received
309 * @from: the node that sent the message
310 * @prio: the priority of the message
312 * This is one of the main functions of the protocol and it provides the
313 * means to receive a message through the protocol stack.
316 int frescan_recv(const frescan_recv_params_t *params,
320 frescan_node_t *from,
321 frescan_prio_t *prio)
324 frescan_prio_queue_t *pqueue;
326 frescan_packet_t *head, *packet;
328 if (params->flags & FRESCAN_SYNC) {
329 DEBUG(FRESCAN_RECV_ENABLE_DEBUG,
330 "receive a packet in blocking mode\n");
333 DEBUG(FRESCAN_RECV_ENABLE_DEBUG,
334 "receive a packet in non-blocking mode\n");
338 pqueue = the_networks[params->net].queues.rx_channel_queues
341 FRESCAN_ACQUIRE_LOCK(&the_networks[params->net].lock);
342 ret = frescan_pqueue_dequeue(pqueue, &head, prio, blocking);
343 FRESCAN_RELEASE_LOCK(&the_networks[params->net].lock);
346 ERROR ("could not dequeue packet\n");
351 if (blocking == false) {
352 DEBUG(FRESCAN_RECV_ENABLE_DEBUG,
353 "blocking false, no packets\n");
357 ERROR ("blocking true, and packet = null\n");
362 DEBUG(FRESCAN_RECV_ENABLE_DEBUG,
363 "traverse the list of packets for this message\n");
367 FRESCAN_ACQUIRE_LOCK(&the_networks[params->net].lock);
369 list_for_each_entry(packet, &head->msg_list, msg_list) {
370 // TODO: memory checks, delete the packets
371 memcpy(msg + *recv_bytes,
374 *recv_bytes += packet->frame->dlc;
376 *from = (frescan_node_t)frescan_id_get_field(packet->frame->id,
378 DEBUG(FRESCAN_RECV_ENABLE_DEBUG,
379 "ID Packet, dlc: %u, frame pool pos: %u, from:%u\n",
380 packet->frame->dlc, packet->frame->pool_pos, *from);
382 ret = can_framespool_free(packet->frame);
384 ERROR("could not free frame\n");
388 ret = frescan_packets_free(packet);
390 ERROR("could not free packet\n");
395 ret = frescan_packets_free(head);
397 FRESCAN_RELEASE_LOCK(&the_networks[params->net].lock);
400 ERROR("could not free head packet\n");
404 DEBUG(FRESCAN_RECV_ENABLE_DEBUG,
405 "received bytes: %u\n", *recv_bytes);
412 * frescan_hook_frame_recv - frame received hook
414 * This function will be called by the CAN driver's IRQ handler when a frame
415 * is received so we can store it in an appropiate queue.
417 * NOTE: in the future it could consist simply of signaling a semaphore to
418 * let a bottom half thread do the hard work.
422 static int frescan_hook_frame_recv (const struct can_chip_t *chip,
423 struct can_frame_t *frame)
426 uint32_t prio, dest, src, channel, frag_id, frag_flag;
427 frescan_packet_t *packet, *head;
428 frescan_prio_queue_t *pqueue;
433 DEBUG(FRESCAN_RX_HOOK_ENABLE_DEBUG, "received a frame, net=%d\n", net);
434 DEBUG(FRESCAN_RX_HOOK_ENABLE_DEBUG || FRESCAN_FRAG_ENABLE_DEBUG,
435 "%s %s, id=0x%X, dlc=%u, pool:%u\n",
436 (frame->is_extended_format) ? "Ext" : "Stnd",
437 (frame->is_rtr) ? "RTR Frame" : "DATA Frame",
442 for (i=0; i<frame->dlc; i++) {
443 DEBUG(FRESCAN_RX_HOOK_ENABLE_DEBUG,
444 "data[%d] = 0x%X;\n", i, frame->data[i]);
447 prio = frescan_id_get_field(frame->id, FRESCAN_FIELD_PRIO);
448 dest = frescan_id_get_field(frame->id, FRESCAN_FIELD_DEST);
449 src = frescan_id_get_field(frame->id, FRESCAN_FIELD_SRC);
450 channel = frescan_id_get_field(frame->id, FRESCAN_FIELD_CHAN);
451 frag_id = frescan_id_get_field(frame->id, FRESCAN_FIELD_FRAG_ID);
452 frag_flag = frescan_id_get_field(frame->id, FRESCAN_FIELD_FRAG_FLAG);
454 DEBUG(FRESCAN_RX_HOOK_ENABLE_DEBUG || FRESCAN_FRAG_ENABLE_DEBUG,
455 "prio:%u dest:%u src:%u chan:%u id:%u flag:%u\n",
456 prio, dest, src, channel, frag_id, frag_flag);
458 DEBUG(FRESCAN_RX_HOOK_ENABLE_DEBUG,
459 "enqueue the packet in ID queue\n");
460 packet = frescan_packets_alloc();
461 packet->frame = frame;
464 head = the_networks[net].id_fp_queues[prio];
466 head = the_networks[net].id_queues[frag_id];
470 DEBUG(FRESCAN_RX_HOOK_ENABLE_DEBUG ||
471 FRESCAN_FRAG_ENABLE_DEBUG,
472 "allocate head for id=%u\n", frag_id);
473 head = frescan_packets_alloc();
474 INIT_LIST_HEAD(&head->msg_list);
477 the_networks[net].id_fp_queues[prio] = head;
479 the_networks[net].id_queues[frag_id] = head;
483 list_add_tail(&packet->msg_list, &head->msg_list);
485 if (frag_flag == false) {
486 DEBUG(FRESCAN_RX_HOOK_ENABLE_DEBUG ||
487 FRESCAN_FRAG_ENABLE_DEBUG,
488 "message complete, move msg to channel\n");
489 // TODO: select the highest priority??
490 pqueue = the_networks[net].queues.rx_channel_queues[channel];
491 ret = frescan_pqueue_enqueue(pqueue, head, prio);
493 ERROR("could not enqueue message in channel queue\n");
498 the_networks[net].id_fp_queues[prio] = NULL;
500 the_networks[net].id_queues[frag_id] = NULL;
504 DEBUG(FRESCAN_RX_HOOK_ENABLE_DEBUG ||
505 FRESCAN_FRAG_ENABLE_DEBUG,
506 "message not complete, wait for more fragments\n");
509 // NOTE: don't forget to free the frame and the packet when it is
510 // read by the user application
516 * frescan_hook_frame_sent - frame sent hook
518 * This function will be called by the CAN driver's IRQ handler when a frame
519 * is sent through the CAN bus so we can enqueue another one, signal a
520 * semaphore, consume sporadic server capacity...
523 static int frescan_hook_frame_sent(const struct can_chip_t *chip)
526 frescan_packet_t *packet;
527 frescan_prio_queue_t *pqueue;
531 packet = the_networks[chip->minor].last_packet;
533 id = frescan_id_get_field(packet->frame->id,
534 FRESCAN_FIELD_FRAG_ID);
536 DEBUG(FRESCAN_SENT_HOOK_ENABLE_DEBUG,
537 "frame sent, minor:%u flags:0x%X frag_id:0x%X\n",
538 chip->minor, packet->flags, id);
540 if (packet->flags & FRESCAN_SS) {
541 DEBUG(FRESCAN_SENT_HOOK_ENABLE_DEBUG,
542 "calling frame_sent + program repl for id:%u\n", id);
544 ret = frescan_replenishment_program(chip->minor, id);
546 ERROR("could not program replenishment\n");
550 ret = frescan_servers_frame_sent(chip->minor, id);
552 ERROR("could not let the server a frame was sent\n");
557 DEBUG(FRESCAN_SENT_HOOK_ENABLE_DEBUG || FRESCAN_FRAG_ENABLE_DEBUG,
558 "last packet buffer_pending_bytes=%u\n",
559 packet->buffer_pending_bytes);
561 if (packet->buffer_pending_bytes > 0) {
562 if (packet->flags & FRESCAN_FP) {
563 prio = frescan_id_get_field(packet->frame->id,
566 DEBUG(FRESCAN_FRAG_ENABLE_DEBUG,
567 "requeue fp packet, prio:%u\n", prio);
569 pqueue = the_networks[chip->minor].queues.tx_fp_queue;
570 ret = frescan_pqueue_requeue(pqueue, packet, prio);
572 ERROR("could not requeue the packet\n");
575 } else if (packet->flags & FRESCAN_SS) {
576 DEBUG(FRESCAN_FRAG_ENABLE_DEBUG,
577 "requeue server %u packet\n", id);
578 ret = frescan_servers_requeue(chip->minor, id, packet);
580 ERROR("could not requeue the packet\n");
584 ERROR("flags are not correct\n");
588 DEBUG(FRESCAN_FRAG_ENABLE_DEBUG,
589 "all packet fragmets sent, freeing the packet\n");
591 ret = can_framespool_free(packet->frame);
593 ERROR ("could not free the frame\n");
597 ret = frescan_packets_free(packet);
599 ERROR ("could not free the packet\n");
603 // TODO: signal semaphore for send_sync
606 the_networks[chip->minor].last_packet = NULL;
608 ret = frescan_hw_buffer_update(chip->minor);
610 ERROR("could not update hw buffer\n");
618 * frescan_hook_frame_aborted - frame frame aborted hook
620 * This function will be called by the CAN driver's IRQ handler when a frame
621 * is aborted (because another frame with higher priority is waiting). We
622 * have to requeue the frame and update the buffer.
625 static int frescan_hook_frame_aborted(const struct can_chip_t *chip)
628 frescan_packet_t *packet;
629 frescan_prio_queue_t *pqueue;
633 packet = the_networks[chip->minor].last_packet;
635 id = frescan_id_get_field(packet->frame->id,
636 FRESCAN_FIELD_FRAG_ID);
638 DEBUG(FRESCAN_SENT_HOOK_ENABLE_DEBUG,
639 "frame aborted, minor:%u flags:0x%X frag_id:0x%X\n",
640 chip->minor, packet->flags, id);
642 if (packet->flags & FRESCAN_FP) {
643 prio = frescan_id_get_field(packet->frame->id,
646 DEBUG(FRESCAN_FRAG_ENABLE_DEBUG,
647 "requeue fp packet, prio:%u\n", prio);
649 pqueue = the_networks[chip->minor].queues.tx_fp_queue;
650 ret = frescan_pqueue_requeue(pqueue, packet, prio);
652 ERROR("could not requeue the packet\n");
655 } else if (packet->flags & FRESCAN_SS) {
656 DEBUG(FRESCAN_FRAG_ENABLE_DEBUG,
657 "requeue server %u packet\n", id);
659 ret = frescan_servers_requeue(chip->minor, id, packet);
661 ERROR("could not requeue the packet\n");
665 ERROR("flags are not correct\n");
669 the_networks[chip->minor].last_packet = NULL;
671 ret = frescan_hw_buffer_update(chip->minor);
673 ERROR("could not update hw buffer\n");