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