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