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 static json_object* create_json_from_openxc_CanMessage(event *event)
 159 {
 160         struct json_object *json;
 161
 162         /*
 163          * TODO: process the openxc_CanMessage struct. Should be a call to a
 164          * decoder function relative to that msg
 165
 166         openxc_CanMessage can_message;
 167         can_message = event->can_message;
 168          */
 169
 170         json = json_object_new_object();
 171         json_object_object_add(json, "name", json_object_new_string(event->name));
 172
 173         return json;
 174 }
 175
 176 /*************************************************************************/
 177 /*************************************************************************/
 178 /**                                                                     **/
 179 /**                                                                     **/
 180 /**        SECTION: HANDLE CAN DEVICE                                   **/
 181 /**                                                                     **/
 182 /**                                                                     **/
 183 /*************************************************************************/
 184 /*************************************************************************/
 185 /*
 186  * open the can socket
 187  */
 188 static int open_can_dev()
 189 {
 190         const int canfd_on = 1;
 191         struct ifreq ifr;
 192         struct timeval timeout = {1,0};
 193
 194         DEBUG(interface, "open_can_dev: CAN Handler socket : %d", can_handler.socket);
 195         if (can_handler.socket >= 0)
 196                 close(can_handler.socket);
 197
 198         can_handler.socket = socket(PF_CAN, SOCK_RAW, CAN_RAW);
 199         if (can_handler.socket < 0)
 200         {
 201                 ERROR(interface, "open_can_dev: socket could not be created");
 202         }
 203         else
 204         {
 205                 /* Set timeout for read */
 206                 setsockopt(can_handler.socket, SOL_SOCKET, SO_RCVTIMEO, (char *)&timeout, sizeof(timeout));
 207                 /* try to switch the socket into CAN_FD mode */
 208                 if (setsockopt(can_handler.socket, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd_on, sizeof(canfd_on)) < 0)
 209                 {
 210                         NOTICE(interface, "open_can_dev: Can not switch into CAN Extended frame format.");
 211                         can_handler.is_fdmode_on = false;
 212                 } else {
 213                         can_handler.is_fdmode_on = true;
 214                 }
 215
 216                 /* Attempts to open a socket to CAN bus */
 217                 strcpy(ifr.ifr_name, can_handler.device);
 218                 if(ioctl(can_handler.socket, SIOCGIFINDEX, &ifr) < 0)
 219                         ERROR(interface, "open_can_dev: ioctl failed");
 220                 else
 221                 {
 222                         can_handler.txAddress.can_family = AF_CAN;
 223                         can_handler.txAddress.can_ifindex = ifr.ifr_ifindex;
 224
 225                         /* And bind it to txAddress */
 226                         if (bind(can_handler.socket, (struct sockaddr *)&can_handler.txAddress, sizeof(can_handler.txAddress)) < 0)
 227                         {
 228                                 ERROR(interface, "open_can_dev: bind failed");
 229                         }
 230                         else
 231                         {
 232                                 fcntl(can_handler.socket, F_SETFL, O_NONBLOCK);
 233                                 return 0;
 234                         }
 235                 }
 236                 close(can_handler.socket);
 237                 can_handler.socket = -1;
 238         }
 239         return -1;
 240 }
 241
 242 /*
 243  * TODO : test that socket is really opened
 244  */
 245 static int write_can()
 246 {
 247         ssize_t nbytes;
 248         int rc;
 249
 250         rc = can_handler.socket;
 251         if (rc >= 0)
 252         {
 253 /*
 254  * TODO change old hvac write can frame to generic on_event
 255  */
 256                 nbytes = sendto(can_handler.socket, &canfd_frame, sizeof(struct canfd_frame), 0,
 257                             (struct sockaddr*)&can_handler.txAddress, sizeof(can_handler.txAddress));
 258                 if (nbytes < 0)
 259                 {
 260                         ERROR(interface, "write_can: Sending CAN frame failed.");
 261                 }
 262         }
 263         else
 264         {
 265                 ERROR(interface, "write_can: socket not initialized. Attempt to reopen can device socket.");
 266                 retry(open_can_dev);
 267         }
 268         return rc;
 269 }
 270
 271 /*
 272  * Parse the CAN frame data payload as a CAN packet
 273  * TODO: parse as an OpenXC Can Message. Don't translate as ASCII and put bytes
 274  * directly into openxc_CanMessage
 275  */
 276 static int parse_can_frame(openxc_CanMessage *can_message, struct canfd_frame *canfd_frame, int maxdlen)
 277 {
 278         int i, len;
 279         //size_t n_msg;
 280
 281         len = (canfd_frame->len > maxdlen) ? maxdlen : canfd_frame->len;
 282
 283         can_message->has_id = true;
 284         if (canfd_frame->can_id & CAN_ERR_FLAG)
 285                 can_message->id = canfd_frame->can_id & (CAN_ERR_MASK|CAN_ERR_FLAG);
 286         else if (canfd_frame->can_id & CAN_EFF_FLAG)
 287         {
 288                 can_message->has_frame_format = true;
 289                 can_message->frame_format = openxc_CanMessage_FrameFormat_EXTENDED;
 290                 can_message->id = canfd_frame->can_id & CAN_EFF_MASK;
 291         } else
 292         {
 293                 can_message->has_frame_format = true;
 294                 can_message->frame_format = openxc_CanMessage_FrameFormat_STANDARD;
 295                 can_message->id = canfd_frame->can_id & CAN_SFF_MASK;
 296         }
 297
 298         /* Don't know what to do with that for now as we haven't
 299          * len fields in openxc_CanMessage struct
 300
 301          * standard CAN frames may have RTR enabled. There are no ERR frames with RTR
 302         if (maxdlen == CAN_MAX_DLEN && canfd_frame->can_id & CAN_RTR_FLAG)
 303         {
 304                 // print a given CAN 2.0B DLC if it's not zero
 305                 if (canfd_frame->len && canfd_frame->len <= CAN_MAX_DLC)
 306                         buf[offset++] = hex_asc_upper[canfd_frame->len & 0xF];
 307
 308                 buf[offset] = 0;
 309                 return NULL;
 310         }
 311         */
 312
 313         /* Doesn't handle real canfd_frame for now
 314         if (maxdlen == CANFD_MAX_DLEN)
 315         {
 316                 // add CAN FD specific escape char and flags
 317                 canfd_frame->flags & 0xF;
 318         } */
 319
 320         if (sizeof(canfd_frame->data) <= sizeof(can_message->data.bytes))
 321         {
 322                 for (i = 0; i < len; i++)
 323                         can_message->data.bytes[i] = canfd_frame->data[i];
 324                 return 0;
 325         } else if (sizeof(canfd_frame->data) <= CAN_MAX_DLEN)
 326         {
 327                 ERROR(interface, "parse_can_frame: can_frame data too long to be stored into openxc_CanMessage data field");
 328                 return -1;
 329                 /* TODO create as many as needed openxc_CanMessage into an array to store all data from canfd_frame
 330                 n_msg = CAN_MAX_DLEN / sizeof(canfd_frame->data.bytes);
 331                 for (i = 0; i < len; i++)
 332                         can_message->data.bytes[i] = canfd_frame->data[i]; */
 333         } else
 334         {
 335                 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 ?");
 336                 return -2;
 337         }
 338
 339         /* You should not reach this return statement */
 340         return -3;
 341 }
 342
 343
 344 /*
 345  * Read on CAN bus and return how much bytes has been read.
 346  */
 347 static int read_can(openxc_CanMessage *can_message)
 348 {
 349         ssize_t nbytes;
 350         int maxdlen;
 351
 352         /* Test that socket is really opened */
 353         if ( socket_test() < 0)
 354         {
 355                 if (retry(open_can_dev) < 0)
 356                 {
 357                         ERROR(interface, "read_can: Socket unavailable");
 358                         return -1;
 359                 }
 360         }
 361
 362         nbytes = read(can_handler.socket, &canfd_frame, CANFD_MTU);
 363
 364         if (nbytes == CANFD_MTU)
 365         {
 366                 DEBUG(interface, "read_can: Got an CAN FD frame with length %d", canfd_frame.len);
 367         }
 368         else if (nbytes == CAN_MTU)
 369         {
 370                 DEBUG(interface, "read_can: Got a legacy CAN frame with length %d", canfd_frame.len);
 371         }
 372         else
 373         {
 374                 if (errno == ENETDOWN)
 375                         ERROR(interface, "read_can: %s interface down", can_handler.device);
 376                 ERROR(interface, "read_can: Error reading CAN bus");
 377                 return -2;
 378         }
 379
 380         /* CAN frame integrity check */
 381         if ((size_t)nbytes == CAN_MTU)
 382                 maxdlen = CAN_MAX_DLEN;
 383         else if ((size_t)nbytes == CANFD_MTU)
 384                 maxdlen = CANFD_MAX_DLEN;
 385         else
 386         {
 387                 ERROR(interface, "read_can: CAN frame incomplete");
 388                 return -3;
 389         }
 390
 391         if (parse_can_frame(can_message, &canfd_frame, maxdlen))
 392         {
 393                 ERROR(interface, "read_can: Can't parse the can frame. ID: %i, DLC: %i, DATA: %s",
 394                       canfd_frame.can_id, canfd_frame.len, canfd_frame.data);
 395                 return -4;
 396         }
 397
 398         return 0;
 399 }
 400 /*************************************************************************/
 401 /*************************************************************************/
 402 /**                                                                     **/
 403 /**                                                                     **/
 404 /**       SECTION: MANAGING EVENTS                                      **/
 405 /**                                                                     **/
 406 /**                                                                     **/
 407 /*************************************************************************/
 408 /*************************************************************************/
 409 static int on_event(sd_event_source *s, int fd, uint32_t revents, void *userdata);
 410
 411 /*
 412  * Get the event loop running.
 413  * Will trigger on_event function on EPOLLIN event on socket
 414  *
 415  * Return 0 or positive value on success. Else negative value for failure.
 416  */
 417 static int connect_to_event_loop()
 418 {
 419         sd_event *event_loop;
 420         sd_event_source *source;
 421         int rc;
 422
 423         if (can_handler.socket < 0)
 424         {
 425                 return can_handler.socket;
 426         }
 427
 428         event_loop = afb_daemon_get_event_loop(interface->daemon);
 429         rc = sd_event_add_io(event_loop, &source, can_handler.socket, EPOLLIN, on_event, NULL);
 430         if (rc < 0)
 431         {
 432                 close(can_handler.socket);
 433                 ERROR(interface, "Can't connect CAN bus %s to the event loop", can_handler.device);
 434         } else
 435         {
 436                 NOTICE(interface, "Connected CAN bus %s to the event loop", can_handler.device);
 437         }
 438
 439         return rc;
 440 }
 441 /*
 442  * Send all events
 443  */
 444 static void send_event()
 445 {
 446         can_event *current;
 447         event *events;
 448         json_object *object;
 449
 450         /* Browse can_events */
 451         current = can_events_list;
 452         while(current)
 453         {
 454                 /* Browse event for each can_events no matter what the id */
 455                 events = current->events;
 456                 while(events)
 457                 {
 458                         object = create_json_from_openxc_CanMessage(events);
 459                         afb_event_push(events->afb_event, object);
 460                         events = events->next;
 461                 }
 462                 current = current->next;
 463         }
 464 }
 465
 466 /*
 467  * called on an event on the CAN bus
 468  */
 469 static int on_event(sd_event_source *s, int fd, uint32_t revents, void *userdata)
 470 {
 471         openxc_CanMessage can_message;
 472
 473         can_message = openxc_CanMessage_init_default;
 474
 475         /* read available data */
 476         if ((revents & EPOLLIN) != 0)
 477         {
 478                 read_can(&can_message);
 479                 send_event();
 480         }
 481
 482         /* check if error or hangup */
 483         if ((revents & (EPOLLERR|EPOLLRDHUP|EPOLLHUP)) != 0)
 484         {
 485                 sd_event_source_unref(s);
 486                 close(fd);
 487                 connect_to_event_loop();
 488         }
 489
 490         return 0;
 491 }
 492
 493 /*
 494  * get or create an event handler for the type
 495  */
 496 static event *get_event(uint32_t id, enum type type)
 497 {
 498         event *event_elt;
 499         can_event *list;
 500
 501         /* find the can list by id */
 502         list = get_event_list_of_id(id);
 503
 504         /* make the new event */
 505         event_elt = (event*)calloc(1, sizeof(event));
 506         event_elt->next = event_elt;
 507         list->events = event_elt;
 508         event_elt->name = create_name(id);
 509         event_elt->afb_event = afb_daemon_make_event(interface->daemon, event_elt->name);
 510
 511         return event_elt;
 512 }
 513
 514 /*************************************************************************/
 515 /*************************************************************************/
 516 /**                                                                     **/
 517 /**                                                                     **/
 518 /**        SECTION: BINDING VERBS IMPLEMENTATION                        **/
 519 /**                                                                     **/
 520 /**                                                                     **/
 521 /*************************************************************************/
 522 /*************************************************************************/
 523 /*
 524  * Returns the type corresponding to the given name
 525  */
 526 static enum type type_of_name(const char *name)
 527 {
 528         enum type result;
 529         if (name == NULL)
 530                 return type_DEFAULT;
 531         for (result = 0 ; (size_t)result < type_size; result++)
 532                 if (strcmp(type_NAMES[result], name) == 0)
 533                         return result;
 534         return type_INVALID;
 535 }
 536
 537 /*
 538  * extract a valid type from the request
 539  */
 540 static int get_type_for_req(struct afb_req req, enum type *type)
 541 {
 542         if ((*type = type_of_name(afb_req_value(req, "type"))) != type_INVALID)
 543                 return 1;
 544         afb_req_fail(req, "unknown-type", NULL);
 545         return 0;
 546 }
 547
 548 /*
 549  * subscribe to notification of new CAN messages
 550  *
 551  * parameters of the subscription are:
 552  *
 553  *    TODO type: string:  choose between CAN and OBDII messages formats.
 554  *
 555  * returns an object with 2 fields:
 556  *
 557  *    name:   string:  the name of the event without its prefix
 558  *    id:     integer: a numeric identifier of the event to be used for unsubscribing
 559  */
 560 static void subscribe(struct afb_req req)
 561 {
 562         enum type type;
 563         event *event;
 564         uint32_t id;
 565         struct json_object *json;
 566
 567         if (get_type_for_req(req, &type))
 568         {
 569                 id = (uint32_t)atoi(afb_req_value(req, "id"));
 570                 event = get_event(id, type);
 571                 if (event == NULL)
 572                         afb_req_fail(req, "out-of-memory", NULL);
 573                 else if (afb_req_subscribe(req, event->afb_event) != 0)
 574                         afb_req_fail_f(req, "failed", "afb_req_subscribe returned an error: %m");
 575                 else
 576                 {
 577                         /* TODO : build json openXC message to send. I guess */
 578                         json = json_object_new_object();
 579                         json_object_object_add(json, "name", json_object_new_string(event->name));
 580                         afb_req_success(req, json, NULL);
 581                 }
 582         }
 583 }
 584
 585 /*
 586  * unsubscribe a previous subscription
 587  *
 588  * parameters of the unsubscription are:
 589  *
 590  *    id:   integer: the numeric identifier of the event as returned when subscribing
 591  */
 592 static void unsubscribe(struct afb_req req)
 593 {
 594         const char *id;
 595         can_event *events_list;
 596         event *event;
 597
 598         id = afb_req_value(req, "id");
 599         if (id == NULL)
 600                 afb_req_fail(req, "missing-id", NULL);
 601         else
 602         {
 603                 events_list = get_event_list_of_id((uint32_t)atoi(id));
 604                 event = events_list->events;
 605                 while(event)
 606                 {
 607                         if (event == NULL)
 608                                 afb_req_fail(req, "bad-id", NULL);
 609                         else
 610                         {
 611                                 afb_req_unsubscribe(req, event->afb_event);
 612                                 afb_req_success(req, NULL, NULL);
 613                         }
 614                         event = event->next;
 615                 }
 616         }
 617 }
 618
 619 static const struct afb_verb_desc_v1 verbs[]=
 620 {
 621   { .name= "subscribe",    .session= AFB_SESSION_NONE, .callback= subscribe,    .info= "subscribe to notification of CAN bus messages." },
 622   { .name= "unsubscribe",  .session= AFB_SESSION_NONE, .callback= unsubscribe,  .info= "unsubscribe a previous subscription." },
 623         {NULL}
 624 };
 625
 626 static const struct afb_binding binding_desc = {
 627         .type = AFB_BINDING_VERSION_1,
 628         .v1 = {
 629                 .info = "CAN bus service",
 630                 .prefix = "can",
 631                 .verbs = verbs
 632         }
 633 };
 634
 635 const struct afb_binding *afbBindingV1Register (const struct afb_binding_interface *itf)
 636 {
 637         interface = itf;
 638
 639         return &binding_desc;
 640 }
 641
 642 int afbBindingV1ServiceInit(struct afb_service service)
 643 {
 644         /* Open CAN socket */
 645         retry(open_can_dev);
 646         return connect_to_event_loop();
 647 }