#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <stdint.h>
 
#include <net/if.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
 
#include <linux/can.h>
#include <linux/can/raw.h>

/**
 * \brief Initializes a raw socket for CAN communication
 * \param ifname the interface, typically can0
 */
int init_can(const char * ifname)
{
  int s;
  struct sockaddr_can addr;
  struct ifreq ifr;
 
  if((s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
    perror("Error while opening socket");
    return -1;
  }
 
  strcpy(ifr.ifr_name, ifname);
  ioctl(s, SIOCGIFINDEX, &ifr);
 
  addr.can_family  = AF_CAN;
  addr.can_ifindex = ifr.ifr_ifindex; 
 
  printf("%s at index %d\n", ifname, ifr.ifr_ifindex);
 
  if(bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
    perror("Error in socket bind");
    return -2;
  }  
  
  return s;
}

/**
 * \brief Writes OD entry
 * \param s socket where to write to
 * \param idx OD index
 * \param subidx OD subindex
 * \param val value to write
 */
int od_write(int s, uint16_t idx, uint8_t subidx, int32_t val)
{
  struct can_frame frame;  
  
  frame.can_id  = 0x600;
  frame.can_dlc = 8;
  frame.data[0] = 0x23;
  frame.data[1] = idx&0xFF;
  frame.data[2] = ( idx >> 8 ) & 0xFF ;
  frame.data[3] = subidx ;
  frame.data[4]  = (   val        & 0xFF);
  frame.data[5]  = ( ( val >> 8 ) & 0xFF);
  frame.data[6]  = ( ( val >> 16 ) & 0xFF);
  frame.data[7]  = ( ( val >> 24 ) & 0xFF);
  
  write(s, &frame, sizeof(struct can_frame));
 
  return 0;
}