canLL-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 #define _GNU_SOURCE
  18
  19 #include <string.h>
  20 #include <stdbool.h>
  21 #include <unistd.h>
  22 #include <sys/types.h>
  23 #include <sys/socket.h>
  24 #include <sys/ioctl.h>
  25 #include <net/if.h>
  26 #include <sys/time.h>
  27 #include <linux/can.h>
  28 #include <linux/can/raw.h>
  29 #include <math.h>
  30 #include <fcntl.h>
  31 #include <systemd/sd-event.h>
  32 #include <errno.h>
  33
  34 #include <json-c/json.h>
  35 #include <openxc.pb.h>
  36
  37 #include <afb/afb-binding.h>
  38 #include <afb/afb-service-itf.h>
  39
  40 /*****************************************************************************************/
  41 /*****************************************************************************************/
  42 /**                                                                                     **/
  43 /**                                                                                     **/
  44 /**        SECTION: GLOBAL VARIABLES                                                    **/
  45 /**                                                                                     **/
  46 /**                                                                                     **/
  47 /*****************************************************************************************/
  48 /*****************************************************************************************/
  49 /* max. number of CAN interfaces given on the cmdline */
  50 #define MAXSOCK 16
  51
  52 /* buffer sizes for CAN frame string representations */
  53 #define CL_ID (sizeof("12345678##1"))
  54 #define CL_DATA sizeof(".AA")
  55 #define CL_BINDATA sizeof(".10101010")
  56
  57  /* CAN FD ASCII hex short representation with DATA_SEPERATORs */
  58 #define CL_CFSZ (2*CL_ID + 64*CL_DATA)
  59
  60 #define CANID_DELIM '#'
  61
  62 /*
  63  * Interface between the daemon and the binding
  64  */
  65 static const struct afb_binding_interface *interface;
  66
  67 /*
  68  * the type of position expected
  69  *
  70  * here, this type is the selection of protocol
  71  */
  72 enum type {
  73         type_OBDII,
  74         type_CAN,
  75         type_DEFAULT = type_CAN,
  76         type_INVALID = -1
  77 };
  78
  79 #define type_size sizeof(enum type)-2
  80
  81 /* CAN variable initialization */
  82 struct canfd_frame canfd_frame;
  83
  84 struct can_handler {
  85         int socket;
  86         char *device;
  87         openxc_CanMessage *msg;
  88         struct sockaddr_can txAddress;
  89 };
  90
  91 /*
  92  * each generated event
  93  */
  94 struct event {
  95         struct event *next;     /* link for the same period */
  96         const char *name;       /* name of the event */
  97         struct afb_event event; /* the event for the binder */
  98         enum type type;         /* the type of data expected */
  99         int id;                 /* id of the event for unsubscribe */
 100 };
 101
 102 /*****************************************************************************************/
 103 /*****************************************************************************************/
 104 /**                                                                                     **/
 105 /**                                                                                     **/
 106 /**        SECTION: UTILITY FUNCTIONS                                                   **/
 107 /**                                                                                     **/
 108 /**                                                                                     **/
 109 /*****************************************************************************************/
 110 /*****************************************************************************************/
 111
 112 /*
 113  * @brief Retry a function 3 times
 114  *
 115  * @param int function(): function that return an int wihtout any parameter
 116  *
 117  * @ return : 0 if ok, -1 if failed
 118  *
 119  */
 120 static int retry( int(*func)());
 121 static int retry( int(*func)())
 122 {
 123         int i;
 124
 125         for (i=0;i<4;i++)
 126         {
 127                 if ( (*func)() >= 0)
 128                 {
 129                         return 0;
 130                 }
 131                 usleep(100000);
 132         }
 133         return -1;
 134 }
 135
 136 /*****************************************************************************************/
 137 /*****************************************************************************************/
 138 /**                                                                                     **/
 139 /**                                                                                     **/
 140 /**        SECTION: HANDLE CAN DEVICE                                                   **/
 141 /**                                                                                     **/
 142 /**                                                                                     **/
 143 /*****************************************************************************************/
 144 /*****************************************************************************************/
 145 const char hex_asc_upper[] = "0123456789ABCDEF";
 146
 147 #define hex_asc_upper_lo(x) hex_asc_upper[((x) & 0x0F)]
 148 #define hex_asc_upper_hi(x) hex_asc_upper[((x) & 0xF0) >> 4]
 149
 150 static inline void put_hex_byte(char *buf, __u8 byte)
 151 {
 152         buf[0] = hex_asc_upper_hi(byte);
 153         buf[1] = hex_asc_upper_lo(byte);
 154 }
 155
 156 static inline void _put_id(char *buf, int end_offset, canid_t id)
 157 {
 158         /* build 3 (SFF) or 8 (EFF) digit CAN identifier */
 159         while (end_offset >= 0) {
 160                 buf[end_offset--] = hex_asc_upper[id & 0xF];
 161                 id >>= 4;
 162         }
 163 }
 164
 165 #define put_sff_id(buf, id) _put_id(buf, 2, id)
 166 #define put_eff_id(buf, id) _put_id(buf, 7, id)
 167
 168 static void canread_frame_parse(struct canfd_frame *canfd_frame, int maxdlen);
 169
 170 /*
 171  * names of the types
 172  */
 173 static const char * const type_NAMES[type_size] = {
 174         "OBDII",
 175         "CAN"
 176 };
 177
 178
 179 // Initialize default can_handler values
 180 static struct can_handler can_handler = {
 181         .socket = -1,
 182         .device = "vcan0",
 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         openxc_CanMessage can_msg = {
 195                 .has_bus = false,
 196                 .has_id = false,
 197                 .has_data = false,
 198                 .has_frame_format = false,
 199         };
 200
 201         DEBUG(interface, "open_can_dev: CAN Handler socket : %d", can_handler.socket);
 202         close(can_handler.socket);
 203
 204         can_handler.socket = socket(PF_CAN, SOCK_RAW, CAN_RAW);
 205         if (can_handler.socket < 0)
 206         {
 207                 ERROR(interface, "open_can_dev: socket could not be created");
 208         }
 209         else
 210         {
 211                 /* Set timeout for read */
 212                 setsockopt(can_handler.socket, SOL_SOCKET, SO_RCVTIMEO, (char *)&timeout, sizeof(timeout));
 213                 /* try to switch the socket into CAN_FD mode */
 214                 can_msg.has_frame_format = true;
 215                 if (setsockopt(can_handler.socket, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd_on, sizeof(canfd_on)) < 0)
 216                 {
 217                         NOTICE(interface, "open_can_dev: Can not switch into CAN Extended frame format.");
 218                         can_msg.frame_format = openxc_CanMessage_FrameFormat_STANDARD;
 219                 } else {
 220                         can_msg.frame_format = openxc_CanMessage_FrameFormat_EXTENDED;
 221                 }
 222
 223                 /* Attempts to open a socket to CAN bus */
 224                 strcpy(ifr.ifr_name, can_handler.device);
 225                 if(ioctl(can_handler.socket, SIOCGIFINDEX, &ifr) < 0)
 226                 {
 227                         ERROR(interface, "open_can_dev: ioctl failed");
 228                 }
 229                 else
 230                 {
 231                         can_handler.txAddress.can_family = AF_CAN;
 232                         can_handler.txAddress.can_ifindex = ifr.ifr_ifindex;
 233
 234                         /* And bind it to txAddress */
 235                         if (bind(can_handler.socket, (struct sockaddr *)&can_handler.txAddress, sizeof(can_handler.txAddress)) < 0)
 236                         {
 237                                 ERROR(interface, "open_can_dev: bind failed");
 238                         }
 239                         else
 240                         {
 241                                 fcntl(can_handler.socket, F_SETFL, O_NONBLOCK);
 242                                 can_handler.msg = &can_msg;
 243                                 return 0;
 244                         }
 245                 }
 246                 close(can_handler.socket);
 247                 can_handler.socket = -1;
 248         }
 249         return -1;
 250 }
 251
 252 static int write_can()
 253 {
 254         ssize_t nbytes;
 255         int rc;
 256
 257         rc = can_handler.socket;
 258         if (rc >= 0)
 259         {
 260 /*
 261  * TODO change old hvac write can frame to generic on_event
 262  */
 263                 nbytes = sendto(can_handler.socket, &canfd_frame, sizeof(struct canfd_frame), 0,
 264                             (struct sockaddr*)&can_handler.txAddress, sizeof(can_handler.txAddress));
 265                 if (nbytes < 0)
 266                 {
 267                         ERROR(interface, "write_can: Sending CAN frame failed.");
 268                 }
 269         }
 270         else
 271         {
 272                 ERROR(interface, "write_can: socket not initialized. Attempt to reopen can device socket.");
 273                 retry(open_can_dev);
 274         }
 275         return rc;
 276 }
 277
 278 /*
 279  * Read on CAN bus and return how much bytes has been read.
 280  */
 281 static int read_can(openxc_CanMessage *can_message)
 282 {
 283         ssize_t nbytes;
 284         int maxdlen;
 285
 286         nbytes = read(can_handler.socket, &canfd_frame, CANFD_MTU);
 287
 288         if (nbytes == CANFD_MTU)
 289         {
 290                 DEBUG(interface, "read_can: Got an CAN FD frame with length %d", canfd_frame.len);
 291         }
 292         else if (nbytes == CAN_MTU)
 293         {
 294                 DEBUG(interface, "read_can: Got a legacy CAN frame with length %d", canfd_frame.len);
 295         }
 296         else
 297         {
 298                 if (errno == ENETDOWN)
 299                         ERROR(interface, "read_can: %s interface down", can_handler.device);
 300                 ERROR(interface, "read_can: Error reading CAN bus");
 301                 return -1;
 302         }
 303
 304         /* CAN frame integrity check */
 305         if ((size_t)nbytes == CAN_MTU)
 306                 maxdlen = CAN_MAX_DLEN;
 307         else if ((size_t)nbytes == CANFD_MTU)
 308                 maxdlen = CANFD_MAX_DLEN;
 309         else
 310         {
 311                 ERROR(interface, "read_can: CAN frame incomplete");
 312                 return -2;
 313         }
 314
 315         canread_frame_parse(&canfd_frame, maxdlen);
 316 }
 317
 318 /*
 319  * Parse the CAN frame data payload as a CAN packet
 320  * TODO: parse as an OpenXC Can Message. Don't translate as ASCII and put bytes
 321  * directly into openxc_CanMessage
 322  */
 323 static void canread_frame_parse(struct canfd_frame *canfd_frame, int maxdlen)
 324 {
 325         int i,offset;
 326         int len = (canfd_frame->len > maxdlen) ? maxdlen : canfd_frame->len;
 327         char buf[CL_CFSZ];
 328
 329         if (canfd_frame->can_id & CAN_ERR_FLAG)
 330         {
 331                 put_eff_id(buf, canfd_frame->can_id & (CAN_ERR_MASK|CAN_ERR_FLAG));
 332                 buf[8] = '#';
 333                 offset = 9;
 334         } else if (canfd_frame->can_id & CAN_EFF_FLAG)
 335         {
 336                 put_eff_id(buf, canfd_frame->can_id & CAN_EFF_MASK);
 337                 buf[8] = '#';
 338                 offset = 9;
 339         } else {
 340                 put_sff_id(buf, canfd_frame->can_id & CAN_SFF_MASK);
 341                 buf[3] = '#';
 342                 offset = 4;
 343         }
 344
 345         /* standard CAN frames may have RTR enabled. There are no ERR frames with RTR */
 346         if (maxdlen == CAN_MAX_DLEN && canfd_frame->can_id & CAN_RTR_FLAG)
 347         {
 348                 buf[offset++] = 'R';
 349                 /* print a given CAN 2.0B DLC if it's not zero */
 350                 if (canfd_frame->len && canfd_frame->len <= CAN_MAX_DLC)
 351                         buf[offset++] = hex_asc_upper[canfd_frame->len & 0xF];
 352
 353                 buf[offset] = 0;
 354                 return;
 355         }
 356
 357         if (maxdlen == CANFD_MAX_DLEN)
 358         {
 359                 /* add CAN FD specific escape char and flags */
 360                 buf[offset++] = '#';
 361                 buf[offset++] = hex_asc_upper[canfd_frame->flags & 0xF];
 362         }
 363
 364         for (i = 0; i < len; i++)
 365         {
 366                 put_hex_byte(buf + offset, canfd_frame->data[i]);
 367                 offset += 2;
 368         }
 369
 370         buf[offset] = 0;
 371         return;
 372 }
 373
 374 /***************************************************************************************/
 375 /***************************************************************************************/
 376 /**                                                                                   **/
 377 /**                                                                                   **/
 378 /**       SECTION: MANAGING EVENTS                                                    **/
 379 /**                                                                                   **/
 380 /**                                                                                   **/
 381 /***************************************************************************************/
 382 /***************************************************************************************/
 383 static int connect_to_event_loop();
 384
 385 /*
 386  * called on an event on the CAN bus
 387  */
 388 static int on_event(sd_event_source *s, int fd, uint32_t revents, void *userdata)
 389 {
 390         openxc_CanMessage can_message;
 391
 392         /* read available data */
 393         if ((revents & EPOLLIN) != 0)
 394         {
 395                 read_can(&can_message);
 396 //              event_send();
 397         }
 398
 399         /* check if error or hangup */
 400         if ((revents & (EPOLLERR|EPOLLRDHUP|EPOLLHUP)) != 0)
 401         {
 402                 sd_event_source_unref(s);
 403                 close(fd);
 404                 connect_to_event_loop();
 405         }
 406
 407         return 0;
 408 }
 409
 410 /*
 411  * get or create an event handler for the type
 412  * TODO: implement function and handle retrieve or create an event as needed
 413  *
 414 static struct event *event_get(enum type type)
 415 {
 416
 417 }
 418 */
 419
 420 static struct event *event_of_id(int id)
 421 {
 422
 423 }
 424
 425 /*****************************************************************************************/
 426 /*****************************************************************************************/
 427 /**                                                                                     **/
 428 /**                                                                                     **/
 429 /**        SECTION: BINDING VERBS IMPLEMENTATION                                        **/
 430 /**                                                                                     **/
 431 /**                                                                                     **/
 432 /*****************************************************************************************/
 433 /*****************************************************************************************/
 434 /*
 435  * Returns the type corresponding to the given name
 436  */
 437 static enum type type_of_name(const char *name)
 438 {
 439         enum type result;
 440         if (name == NULL)
 441                 return type_DEFAULT;
 442         for (result = 0 ; result < type_size; result++)
 443                 if (strcmp(type_NAMES[result], name) == 0)
 444                         return result;
 445         return type_INVALID;
 446 }
 447
 448 /*
 449  * extract a valid type from the request
 450  */
 451 static int get_type_for_req(struct afb_req req, enum type *type)
 452 {
 453         if ((*type = type_of_name(afb_req_value(req, "type"))) != type_INVALID)
 454                 return 1;
 455         afb_req_fail(req, "unknown-type", NULL);
 456         return 0;
 457 }
 458
 459 /*
 460  * subscribe to notification of new CAN messages
 461  *
 462  * parameters of the subscription are:
 463  *
 464  *    TODO type: string:  choose between CAN and OBDII messages formats.
 465  *
 466  * returns an object with 2 fields:
 467  *
 468  *    name:   string:  the name of the event without its prefix
 469  *    id:     integer: a numeric identifier of the event to be used for unsubscribing
 470  */
 471 static void subscribe(struct afb_req req)
 472 {
 473         enum type type;
 474         const char *period;
 475         struct event *can_sig;
 476         struct json_object *json;
 477
 478         if (get_type_for_req(req, &type))
 479         {
 480                 can_sig->event = afb_daemon_make_event(interface->daemon, type_NAMES[type]);
 481                 if (can_sig == NULL)
 482                         afb_req_fail(req, "out-of-memory", NULL);
 483                 else if (afb_req_subscribe(req, can_sig->event) != 0)
 484                         afb_req_fail_f(req, "failed", "afb_req_subscribe returned an error: %m");
 485                 else
 486                 {
 487                         json = json_object_new_object();
 488                         json_object_object_add(json, "name", json_object_new_string(event->name));
 489                         json_object_object_add(json, "id", json_object_new_int(event->id));
 490                         afb_req_success(req, json, NULL);
 491                 }
 492         }
 493 }
 494
 495 /*
 496  * unsubscribe a previous subscription
 497  *
 498  * parameters of the unsubscription are:
 499  *
 500  *    id:   integer: the numeric identifier of the event as returned when subscribing
 501  */
 502 static void unsubscribe(struct afb_req req)
 503 {
 504         const char *id;
 505         struct event *event;
 506
 507         id = afb_req_value(req, "id");
 508         if (id == NULL)
 509                 afb_req_fail(req, "missing-id", NULL);
 510         else
 511         {
 512                 event = event_of_id(atoi(id));
 513                 if (event == NULL)
 514                         afb_req_fail(req, "bad-id", NULL);
 515                 else
 516                 {
 517                         afb_req_unsubscribe(req, event->event);
 518                         afb_req_success(req, NULL, NULL);
 519                 }
 520         }
 521 }
 522
 523 static int connect_to_event_loop()
 524 {
 525         sd_event_source *source;
 526         int rc;
 527
 528         retry(open_can_dev);
 529
 530         if (can_handler.socket < 0)
 531         {
 532                 return can_handler.socket;
 533         }
 534
 535         rc = sd_event_add_io(afb_daemon_get_event_loop(interface->daemon), &source, can_handler.socket, EPOLLIN, on_event, NULL);
 536         if (rc < 0)
 537         {
 538                 close(can_handler.socket);
 539                 ERROR(interface, "Can't connect CAN bus %s to the event loop", can_handler.device);
 540         } else
 541         {
 542                 NOTICE(interface, "Connected CAN bus %s to the event loop", can_handler.device);
 543         }
 544
 545         return rc;
 546 }
 547
 548
 549 // TODO: Have to change session management flag to AFB_SESSION_CHECK to use token auth
 550 static const struct afb_verb_desc_v1 verbs[]=
 551 {
 552   { .name= "subscribe",    .session= AFB_SESSION_NONE, .callback= subscribe,    .info= "subscribe to notification of CAN bus messages." },
 553   { .name= "unsubscribe",  .session= AFB_SESSION_NONE, .callback= unsubscribe,  .info= "unsubscribe a previous subscription." },
 554         {NULL}
 555 };
 556
 557 static const struct afb_binding binding_desc = {
 558         .type = AFB_BINDING_VERSION_1,
 559         .v1 = {
 560                 .info = "CAN bus service",
 561                 .prefix = "can",
 562                 .verbs = verbs
 563         }
 564 };
 565
 566 const struct afb_binding *afbBindingV1Register (const struct afb_binding_interface *itf)
 567 {
 568         interface = itf;
 569
 570         return &binding_desc;
 571 }
 572
 573 int afbBindingV1ServiceInit(struct afb_service service)
 574 {
 575         return connect_to_event_loop();
 576 }