obsolete/ll-can-binding.cpp

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