ll-can-binding.cpp

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