ll-can-binding.c

   1 /*
   2  * Copyright (C) 2015, 2016 "IoT.bzh"
   3  * Author "Romain Forlot" <romain.forlot@iot.bzh>
   4  *
   5  * Licensed under the Apache License, Version 2.0 (the "License");
   6  * you may not use this file except in compliance with the License.
   7  * You may obtain a copy of the License at
   8  *
   9  *   http://www.apache.org/licenses/LICENSE-2.0
  10  *
  11  * Unless required by applicable law or agreed to in writing, software
  12  * distributed under the License is distributed on an "AS IS" BASIS,
  13  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14  * See the License for the specific language governing permissions and
  15  * limitations under the License.
  16  */
  17
  18 #define _GNU_SOURCE
  19
  20 #include <string.h>
  21 #include <stdbool.h>
  22 #include <unistd.h>
  23 #include <sys/types.h>
  24 #include <sys/socket.h>
  25 #include <sys/ioctl.h>
  26 #include <net/if.h>
  27 #include <sys/time.h>
  28 #include <linux/can.h>
  29 #include <linux/can/raw.h>
  30 #include <math.h>
  31 #include <fcntl.h>
  32 #include <systemd/sd-event.h>
  33 #include <errno.h>
  34
  35 #include <json-c/json.h>
  36 #include <openxc.pb.h>
  37
  38 #include <afb/afb-binding.h>
  39 #include <afb/afb-service-itf.h>
  40
  41 #include "ll-can-binding.h"
  42
  43 /*************************************************************************/
  44 /*************************************************************************/
  45 /**                                                                     **/
  46 /**                                                                     **/
  47 /**        SECTION: UTILITY FUNCTIONS                                   **/
  48 /**                                                                     **/
  49 /**                                                                     **/
  50 /*************************************************************************/
  51 /*************************************************************************/
  52
  53 /*
  54  * Retry a function 3 times
  55  *
  56  * param int function(): function that return an int wihtout any parameter
  57  *
  58  * return : 0 if ok, -1 if failed
  59  *
  60  */
  61 static int retry( int(*func)())
  62 {
  63         int i;
  64
  65         for (i=0;i<4;i++)
  66         {
  67                 if ( (*func)() >= 0)
  68                 {
  69                         return 0;
  70                 }
  71                 usleep(100000);
  72         }
  73         return -1;
  74 }
  75
  76 /*
  77  * Test that socket is really opened
  78  *
  79  * param
  80  *
  81  * return : 0 or positive int if ok, negative value if failed
  82  *
  83  */
  84 static int socket_test()
  85 {
  86         if (can_handler.socket < 0)
  87         {
  88                 return -1;
  89         }
  90         return 0;
  91 }
  92
  93 /*
  94  * Browse chained list and return the one with specified id
  95  *
  96  * param uint32_t id : can arbitration identifier
  97  *
  98  * return can_event
  99  */
 100 static can_event *get_event_list_of_id(uint32_t id)
 101 {
 102         can_event *current;
 103
 104         /* create and return if lists not exists */
 105         if (!can_events_list)
 106         {
 107                 can_events_list = (can_event*)calloc(1, sizeof(can_event));
 108                 can_events_list->id = id;
 109                 return can_events_list;
 110         }
 111
 112         /* search for id */
 113         current = can_events_list;
 114         while(current)
 115         {
 116                 if (current->id == id)
 117                         return current;
 118                 if (!current->next)
 119                 {
 120                         current->next = (can_event*)calloc(1, sizeof(can_event));
 121                         current->next->id = id;
 122                         return current->next;
 123                 }
 124                 current = current->next;
 125         }
 126
 127         return NULL;
 128 }
 129
 130 /*
 131  * Take an id and return it into a char array
 132  */
 133 static char* create_name(uint32_t id)
 134 {
 135         char name[32];
 136         size_t nchar;
 137
 138         nchar = (size_t)sprintf(name, "can_%u", id);
 139         if (nchar > 0)
 140         {
 141                 char *result = (char*)malloc(nchar + 1);
 142                 memcpy(result, name, nchar);
 143                 result[nchar] = 0;
 144                 return result;
 145         }
 146
 147         return NULL;
 148 }
 149
 150 /*
 151  * Create json object that will be pushed through event_loop to any subscriber
 152  *
 153  *  param : openxc_CanMessage structure complete with data to put into json
 154  *  object.
 155  *
 156  *  return : json object
 157  *
 158  *
 159  */
 160 static json_object* create_json_from_openxc_CanMessage(event *event)
 161 {
 162         struct json_object *json;
 163
 164         /*
 165          * TODO: process the openxc_CanMessage struct. Should be a call to a
 166          * decoder function relative to that msg
 167
 168         openxc_CanMessage can_message;
 169         can_message = event->can_message;
 170          */
 171
 172         json = json_object_new_object();
 173         json_object_object_add(json, "name", json_object_new_string(event->name));
 174
 175         return json;
 176 }
 177
 178 /*************************************************************************/
 179 /*************************************************************************/
 180 /**                                                                     **/
 181 /**                                                                     **/
 182 /**        SECTION: HANDLE CAN DEVICE                                   **/
 183 /**                                                                     **/
 184 /**                                                                     **/
 185 /*************************************************************************/
 186 /*************************************************************************/
 187 /*
 188  * open the can socket
 189  */
 190 static int open_can_dev()
 191 {
 192         const int canfd_on = 1;
 193         struct ifreq ifr;
 194         struct timeval timeout = {1,0};
 195
 196         DEBUG(interface, "open_can_dev: CAN Handler socket : %d", can_handler.socket);
 197         if (can_handler.socket >= 0)
 198                 close(can_handler.socket);
 199
 200         can_handler.socket = socket(PF_CAN, SOCK_RAW, CAN_RAW);
 201         if (can_handler.socket < 0)
 202         {
 203                 ERROR(interface, "open_can_dev: socket could not be created");
 204         }
 205         else
 206         {
 207                 /* Set timeout for read */
 208                 setsockopt(can_handler.socket, SOL_SOCKET, SO_RCVTIMEO, (char *)&timeout, sizeof(timeout));
 209                 /* try to switch the socket into CAN_FD mode */
 210                 if (setsockopt(can_handler.socket, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd_on, sizeof(canfd_on)) < 0)
 211                 {
 212                         NOTICE(interface, "open_can_dev: Can not switch into CAN Extended frame format.");
 213                         can_handler.is_fdmode_on = false;
 214                 } else {
 215                         can_handler.is_fdmode_on = true;
 216                 }
 217
 218                 /* Attempts to open a socket to CAN bus */
 219                 strcpy(ifr.ifr_name, can_handler.device);
 220                 if(ioctl(can_handler.socket, SIOCGIFINDEX, &ifr) < 0)
 221                         ERROR(interface, "open_can_dev: ioctl failed");
 222                 else
 223                 {
 224                         can_handler.txAddress.can_family = AF_CAN;
 225                         can_handler.txAddress.can_ifindex = ifr.ifr_ifindex;
 226
 227                         /* And bind it to txAddress */
 228                         if (bind(can_handler.socket, (struct sockaddr *)&can_handler.txAddress, sizeof(can_handler.txAddress)) < 0)
 229                         {
 230                                 ERROR(interface, "open_can_dev: bind failed");
 231                         }
 232                         else
 233                         {
 234                                 fcntl(can_handler.socket, F_SETFL, O_NONBLOCK);
 235                                 return 0;
 236                         }
 237                 }
 238                 close(can_handler.socket);
 239                 can_handler.socket = -1;
 240         }
 241         return -1;
 242 }
 243
 244 /*
 245  * TODO : test that socket is really opened
 246  */
 247 static int write_can()
 248 {
 249         ssize_t nbytes;
 250         int rc;
 251
 252         rc = can_handler.socket;
 253         if (rc >= 0)
 254         {
 255 /*
 256  * TODO change old hvac write can frame to generic on_event
 257  */
 258                 nbytes = sendto(can_handler.socket, &canfd_frame, sizeof(struct canfd_frame), 0,
 259                             (struct sockaddr*)&can_handler.txAddress, sizeof(can_handler.txAddress));
 260                 if (nbytes < 0)
 261                 {
 262                         ERROR(interface, "write_can: Sending CAN frame failed.");
 263                 }
 264         }
 265         else
 266         {
 267                 ERROR(interface, "write_can: socket not initialized. Attempt to reopen can device socket.");
 268                 retry(open_can_dev);
 269         }
 270         return rc;
 271 }
 272
 273 /*
 274  * Read on CAN bus and return how much bytes has been read.
 275  */
 276 static int read_can(openxc_CanMessage *can_message)
 277 {
 278         ssize_t nbytes;
 279         int maxdlen;
 280
 281         /* Test that socket is really opened */
 282         if ( socket_test() < 0)
 283         {
 284                 if (retry(open_can_dev) < 0)
 285                 {
 286                         ERROR(interface, "read_can: Socket unavailable");
 287                         return -1;
 288                 }
 289         }
 290
 291         nbytes = read(can_handler.socket, &canfd_frame, CANFD_MTU);
 292
 293         if (nbytes == CANFD_MTU)
 294         {
 295                 DEBUG(interface, "read_can: Got an CAN FD frame with length %d", canfd_frame.len);
 296         }
 297         else if (nbytes == CAN_MTU)
 298         {
 299                 DEBUG(interface, "read_can: Got a legacy CAN frame with length %d", canfd_frame.len);
 300         }
 301         else
 302         {
 303                 if (errno == ENETDOWN)
 304                         ERROR(interface, "read_can: %s interface down", can_handler.device);
 305                 ERROR(interface, "read_can: Error reading CAN bus");
 306                 return -2;
 307         }
 308
 309         /* CAN frame integrity check */
 310         if ((size_t)nbytes == CAN_MTU)
 311                 maxdlen = CAN_MAX_DLEN;
 312         else if ((size_t)nbytes == CANFD_MTU)
 313                 maxdlen = CANFD_MAX_DLEN;
 314         else
 315         {
 316                 ERROR(interface, "read_can: CAN frame incomplete");
 317                 return -3;
 318         }
 319
 320         if (parse_can_frame(can_message, &canfd_frame, maxdlen))
 321         {
 322                 ERROR(interface, "read_can: Can't parse the can frame. ID: %i, DLC: %i, DATA: %s",
 323                       canfd_frame.can_id, canfd_frame.len, canfd_frame.data);
 324                 return -4;
 325         }
 326
 327         return 0;
 328 }
 329
 330 /*
 331  * Parse the CAN frame data payload as a CAN packet
 332  * TODO: parse as an OpenXC Can Message. Don't translate as ASCII and put bytes
 333  * directly into openxc_CanMessage
 334  */
 335 static int parse_can_frame(openxc_CanMessage *can_message, struct canfd_frame *canfd_frame, int maxdlen)
 336 {
 337         int i, len;
 338         //size_t n_msg;
 339
 340         len = (canfd_frame->len > maxdlen) ? maxdlen : canfd_frame->len;
 341
 342         can_message->has_id = true;
 343         if (canfd_frame->can_id & CAN_ERR_FLAG)
 344                 can_message->id = canfd_frame->can_id & (CAN_ERR_MASK|CAN_ERR_FLAG);
 345         else if (canfd_frame->can_id & CAN_EFF_FLAG)
 346         {
 347                 can_message->has_frame_format = true;
 348                 can_message->frame_format = openxc_CanMessage_FrameFormat_EXTENDED;
 349                 can_message->id = canfd_frame->can_id & CAN_EFF_MASK;
 350         } else
 351         {
 352                 can_message->has_frame_format = true;
 353                 can_message->frame_format = openxc_CanMessage_FrameFormat_STANDARD;
 354                 can_message->id = canfd_frame->can_id & CAN_SFF_MASK;
 355         }
 356
 357         /* Don't know what to do with that for now as we haven't
 358          * len fields in openxc_CanMessage struct
 359
 360          * standard CAN frames may have RTR enabled. There are no ERR frames with RTR
 361         if (maxdlen == CAN_MAX_DLEN && canfd_frame->can_id & CAN_RTR_FLAG)
 362         {
 363                 // print a given CAN 2.0B DLC if it's not zero
 364                 if (canfd_frame->len && canfd_frame->len <= CAN_MAX_DLC)
 365                         buf[offset++] = hex_asc_upper[canfd_frame->len & 0xF];
 366
 367                 buf[offset] = 0;
 368                 return NULL;
 369         }
 370         */
 371
 372         /* Doesn't handle real canfd_frame for now
 373         if (maxdlen == CANFD_MAX_DLEN)
 374         {
 375                 // add CAN FD specific escape char and flags
 376                 canfd_frame->flags & 0xF;
 377         } */
 378
 379         if (sizeof(canfd_frame->data) <= sizeof(can_message->data.bytes))
 380         {
 381                 for (i = 0; i < len; i++)
 382                         can_message->data.bytes[i] = canfd_frame->data[i];
 383                 return 0;
 384         } else if (sizeof(canfd_frame->data) <= CAN_MAX_DLEN)
 385         {
 386                 ERROR(interface, "parse_can_frame: can_frame data too long to be stored into openxc_CanMessage data field");
 387                 return -1;
 388                 /* TODO create as many as needed openxc_CanMessage into an array to store all data from canfd_frame
 389                 n_msg = CAN_MAX_DLEN / sizeof(canfd_frame->data.bytes);
 390                 for (i = 0; i < len; i++)
 391                         can_message->data.bytes[i] = canfd_frame->data[i]; */
 392         } else
 393         {
 394                 ERROR(interface, "parse_can_frame: can_frame is really too long here. Size of data greater than canfd maximum 64bytes size. Is it a CAN message ?");
 395                 return -2;
 396         }
 397
 398         /* You should not reach this return statement */
 399         return -3;
 400 }
 401
 402 /*************************************************************************/
 403 /*************************************************************************/
 404 /**                                                                     **/
 405 /**                                                                     **/
 406 /**       SECTION: MANAGING EVENTS                                      **/
 407 /**                                                                     **/
 408 /**                                                                     **/
 409 /*************************************************************************/
 410 /*************************************************************************/
 411 /*
 412  * called on an event on the CAN bus
 413  */
 414 static int on_event(sd_event_source *s, int fd, uint32_t revents, void *userdata)
 415 {
 416         openxc_CanMessage can_message;
 417
 418         can_message = openxc_CanMessage_init_default;
 419
 420         /* read available data */
 421         if ((revents & EPOLLIN) != 0)
 422         {
 423                 read_can(&can_message);
 424                 send_event();
 425         }
 426
 427         /* check if error or hangup */
 428         if ((revents & (EPOLLERR|EPOLLRDHUP|EPOLLHUP)) != 0)
 429         {
 430                 sd_event_source_unref(s);
 431                 close(fd);
 432                 connect_to_event_loop();
 433         }
 434
 435         return 0;
 436 }
 437
 438 /*
 439  * get or create an event handler for the type
 440  */
 441 static event *get_event(uint32_t id, enum type type)
 442 {
 443         event *event_elt;
 444         can_event *list;
 445
 446         /* find the can list by id */
 447         list = get_event_list_of_id(id);
 448
 449         /* make the new event */
 450         event_elt = (event*)calloc(1, sizeof(event));
 451         event_elt->next = event_elt;
 452         list->events = event_elt;
 453         event_elt->name = create_name(id);
 454         event_elt->afb_event = afb_daemon_make_event(interface->daemon, event_elt->name);
 455
 456         return event_elt;
 457 }
 458
 459 /*
 460  * Send all events
 461  */
 462 static void send_event()
 463 {
 464         can_event *current;
 465         event *events;
 466         json_object *object;
 467
 468         /* Browse can_events */
 469         current = can_events_list;
 470         while(current)
 471         {
 472                 /* Browse event for each can_events no matter what the id */
 473                 events = current->events;
 474                 while(events)
 475                 {
 476                         object = create_json_from_openxc_CanMessage(events);
 477                         afb_event_push(events->afb_event, object);
 478                         events = events->next;
 479                 }
 480                 current = current->next;
 481         }
 482 }
 483
 484 /*
 485  * Get the event loop running.
 486  * Will trigger on_event function on EPOLLIN event on socket
 487  *
 488  * Return 0 or positive value on success. Else negative value for failure.
 489  */
 490 static int connect_to_event_loop()
 491 {
 492         sd_event *event_loop;
 493         sd_event_source *source;
 494         int rc;
 495
 496         if (can_handler.socket < 0)
 497         {
 498                 return can_handler.socket;
 499         }
 500
 501         event_loop = afb_daemon_get_event_loop(interface->daemon);
 502         rc = sd_event_add_io(event_loop, &source, can_handler.socket, EPOLLIN, on_event, NULL);
 503         if (rc < 0)
 504         {
 505                 close(can_handler.socket);
 506                 ERROR(interface, "Can't connect CAN bus %s to the event loop", can_handler.device);
 507         } else
 508         {
 509                 NOTICE(interface, "Connected CAN bus %s to the event loop", can_handler.device);
 510         }
 511
 512         return rc;
 513 }
 514
 515 /*************************************************************************/
 516 /*************************************************************************/
 517 /**                                                                     **/
 518 /**                                                                     **/
 519 /**        SECTION: BINDING VERBS IMPLEMENTATION                        **/
 520 /**                                                                     **/
 521 /**                                                                     **/
 522 /*************************************************************************/
 523 /*************************************************************************/
 524 /*
 525  * Returns the type corresponding to the given name
 526  */
 527 static enum type type_of_name(const char *name)
 528 {
 529         enum type result;
 530         if (name == NULL)
 531                 return type_DEFAULT;
 532         for (result = 0 ; (size_t)result < type_size; result++)
 533                 if (strcmp(type_NAMES[result], name) == 0)
 534                         return result;
 535         return type_INVALID;
 536 }
 537
 538 /*
 539  * extract a valid type from the request
 540  */
 541 static int get_type_for_req(struct afb_req req, enum type *type)
 542 {
 543         if ((*type = type_of_name(afb_req_value(req, "type"))) != type_INVALID)
 544                 return 1;
 545         afb_req_fail(req, "unknown-type", NULL);
 546         return 0;
 547 }
 548
 549 /*
 550  * subscribe to notification of new CAN messages
 551  *
 552  * parameters of the subscription are:
 553  *
 554  *    TODO type: string:  choose between CAN and OBDII messages formats.
 555  *
 556  * returns an object with 2 fields:
 557  *
 558  *    name:   string:  the name of the event without its prefix
 559  *    id:     integer: a numeric identifier of the event to be used for unsubscribing
 560  */
 561 static void subscribe(struct afb_req req)
 562 {
 563         enum type type;
 564         event *event;
 565         uint32_t id;
 566         struct json_object *json;
 567
 568         if (get_type_for_req(req, &type))
 569         {
 570                 id = (uint32_t)atoi(afb_req_value(req, "id"));
 571                 event = get_event(id, type);
 572                 if (event == NULL)
 573                         afb_req_fail(req, "out-of-memory", NULL);
 574                 else if (afb_req_subscribe(req, event->afb_event) != 0)
 575                         afb_req_fail_f(req, "failed", "afb_req_subscribe returned an error: %m");
 576                 else
 577                 {
 578                         /* TODO : build json openXC message to send. I guess */
 579                         json = json_object_new_object();
 580                         json_object_object_add(json, "name", json_object_new_string(event->name));
 581                         afb_req_success(req, json, NULL);
 582                 }
 583         }
 584 }
 585
 586 /*
 587  * unsubscribe a previous subscription
 588  *
 589  * parameters of the unsubscription are:
 590  *
 591  *    id:   integer: the numeric identifier of the event as returned when subscribing
 592  */
 593 static void unsubscribe(struct afb_req req)
 594 {
 595         const char *id;
 596         can_event *events_list;
 597         event *event;
 598
 599         id = afb_req_value(req, "id");
 600         if (id == NULL)
 601                 afb_req_fail(req, "missing-id", NULL);
 602         else
 603         {
 604                 events_list = get_event_list_of_id((uint32_t)atoi(id));
 605                 event = events_list->events;
 606                 while(event)
 607                 {
 608                         if (event == NULL)
 609                                 afb_req_fail(req, "bad-id", NULL);
 610                         else
 611                         {
 612                                 afb_req_unsubscribe(req, event->afb_event);
 613                                 afb_req_success(req, NULL, NULL);
 614                         }
 615                         event = event->next;
 616                 }
 617         }
 618 }
 619
 620 // TODO: Have to change session management flag to AFB_SESSION_CHECK to use token auth
 621 static const struct afb_verb_desc_v1 verbs[]=
 622 {
 623   { .name= "subscribe",    .session= AFB_SESSION_NONE, .callback= subscribe,    .info= "subscribe to notification of CAN bus messages." },
 624   { .name= "unsubscribe",  .session= AFB_SESSION_NONE, .callback= unsubscribe,  .info= "unsubscribe a previous subscription." },
 625         {NULL}
 626 };
 627
 628 static const struct afb_binding binding_desc = {
 629         .type = AFB_BINDING_VERSION_1,
 630         .v1 = {
 631                 .info = "CAN bus service",
 632                 .prefix = "can",
 633                 .verbs = verbs
 634         }
 635 };
 636
 637 const struct afb_binding *afbBindingV1Register (const struct afb_binding_interface *itf)
 638 {
 639         interface = itf;
 640
 641         return &binding_desc;
 642 }
 643
 644 int afbBindingV1ServiceInit(struct afb_service service)
 645 {
 646         /* Open CAN socket */
 647         retry(open_can_dev);
 648         return connect_to_event_loop();
 649 }