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