//---------------------------------------------------------------
//
//  e c _ m a s t e r . c
//
//  $LastChangedDate$
//  $Author$
//
//---------------------------------------------------------------

#include "ec_globals.h"
#include "ec_master.h"

#define DEBUG_SEND_RECEIVE

//---------------------------------------------------------------

int EtherCAT_master_init(EtherCAT_master_t *master,
                         char *eth_dev)
{
  char errbuf_libpcap[PCAP_ERRBUF_SIZE];
  char errbuf_libnet[LIBNET_ERRBUF_SIZE];

  master->slaves = NULL;
  master->slave_count = 0;

  master->first_command = NULL;
  master->command_index = 0x00;

  master->process_data = NULL;
  master->pre_cb = NULL;
  master->post_cb = NULL;
  master->thread_continue = 0;
  master->cycle_time = 0;
  
  // Init Libpcap
  master->pcap_handle = pcap_open_live(eth_dev, BUFSIZ, 1, 0, errbuf_libpcap);

  if (master->pcap_handle == NULL)
  {
    fprintf(stderr, "Couldn't open device %s: %s\n", eth_dev, errbuf_libpcap);
    return 1;
  }

  // Init Libnet
  if (!(master->net_handle = libnet_init(LIBNET_LINK, eth_dev, errbuf_libnet)))
  {
    fprintf(stderr, "Could not init %s: %s!\n", eth_dev, errbuf_libnet);

    pcap_close(master->pcap_handle);

    return 1;
  }

  return 0;
}

//---------------------------------------------------------------

void EtherCAT_master_clear(EtherCAT_master_t *master)
{
  libnet_destroy(master->net_handle);
  pcap_close(master->pcap_handle);

  // Remove all pending commands
  while (master->first_command)
  {
    EtherCAT_remove_command(master, master->first_command);
  }

  // Remove all slaves
  EtherCAT_clear_slaves(master);
}

//---------------------------------------------------------------

int EtherCAT_check_slaves(EtherCAT_master_t *master,
                          EtherCAT_slave_t *slaves,
                          unsigned int slave_count)
{
  EtherCAT_command_t *cmd;
  EtherCAT_slave_t *cur;
  unsigned int i, j, found;
  unsigned char data[2];

  // EtherCAT_clear_slaves() must be called before!
  if (master->slave_count) return -1;

  // Determine number of slaves

  if ((cmd = EtherCAT_broadcast_read(master, 0x0000, 4)) == NULL)
  {
    return -1;
  }
  
  if (EtherCAT_send_receive(master) != 0)
  {
    return -1;
  }

  master->slave_count = cmd->working_counter;

  EtherCAT_remove_command(master, cmd);

  if (master->slave_count < slave_count)
  {
    fprintf(stderr, "ERROR: Too few slaves on EtherCAT bus!\n");
    return -1;
  }

  // No slaves. Stop further processing...
  if (master->slave_count == 0) return 0;

   // For every slave in the list
  for (i = 0; i < master->slave_count; i++)
  {
    cur = &slaves[i];

    if (!cur->desc)
    {
      fprintf(stderr, "ERROR: Slave has no description (list position %i)!\n", i);
      return -1;
    }

    // Set ring position
    cur->ring_position = -i;
    cur->station_address = i + 1;

    // Write station address
    
    data[0] = cur->station_address & 0x00FF;
    data[1] = (cur->station_address & 0xFF00) >> 8;

    if ((cmd = EtherCAT_position_write(master, cur->ring_position, 0x0010, 2, data)) == NULL)
    {
      master->slave_count = 0;
      fprintf(stderr, "ERROR: Could not create command!\n");
      return -1;
    }
    
    if (EtherCAT_send_receive(master) != 0)
    {
      master->slave_count = 0;
      fprintf(stderr, "ERROR: Could not send command!\n");
      return -1;
    }
    
    if (cmd->working_counter != 1)
    {
      master->slave_count = 0;
      fprintf(stderr, "ERROR: Slave %i did not repond while writing station address!\n", i);
      return -1;
    }
    
    EtherCAT_remove_command(master, cmd);

    // Read base data

    if ((cmd = EtherCAT_read(master, cur->station_address, 0x0000, 4)) == NULL)
    {
      master->slave_count = 0;
      fprintf(stderr, "ERROR: Could not create command!\n");
      return -1;
    }

    if (EtherCAT_send_receive(master) != 0)
    {
      EtherCAT_remove_command(master, cmd);
      master->slave_count = 0;
      fprintf(stderr, "ERROR: Could not send command!\n");
      return -4;
    }

    if (cmd->working_counter != 1)
    {
      EtherCAT_remove_command(master, cmd);
      master->slave_count = 0;
      fprintf(stderr, "ERROR: Slave %i did not respond while reading base data!\n", i);
      return -5;
    }

    // Get base data
    cur->type = cmd->data[0];
    cur->revision = cmd->data[1];
    cur->build = cmd->data[2] | (cmd->data[3] << 8);

    EtherCAT_remove_command(master, cmd);

    // Read identification from "Slave Information Interface" (SII)

    if (EtherCAT_read_slave_information(master, cur->station_address, 0x0008, &cur->vendor_id) != 0)
    {
      master->slave_count = 0;
      fprintf(stderr, "ERROR: Could not read SII!\n", i);
      return -1;
    }

    if (EtherCAT_read_slave_information(master, cur->station_address, 0x000A, &cur->product_code) != 0)
    {
      master->slave_count = 0;
      fprintf(stderr, "ERROR: Could not read SII!\n", i);
      return -1;
    }

    if (EtherCAT_read_slave_information(master, cur->station_address, 0x000E, &cur->revision_number) != 0)
    {
      master->slave_count = 0;
      fprintf(stderr, "ERROR: Could not read SII!\n", i);
      return -1;
    }

    // Search for identification in "database"

    found = 0;

    for (j = 0; j < slave_idents_count; j++)
    {
      if (slave_idents[j].vendor_id == cur->vendor_id
          && slave_idents[j].product_code == cur->product_code)
      {
        found = 1;

        if (cur->desc != slave_idents[j].desc)
        {
          fprintf(stderr, "ERROR: Unexpected slave device at position %i:"
                  "%s %s. Expected: %s %s\n",
                  i, slave_idents[j].desc->vendor_name, slave_idents[j].desc->product_name,
                  cur->desc->vendor_name, cur->desc->product_name);
          return -1;
        }

        break;
      }
    }

    if (!found)
    {
      fprintf(stderr, "ERROR: Unknown slave device at position %i: Vendor %X, Code %X",
              i, cur->vendor_id, cur->product_code);
      return -1;      
    }
  }

  return 0;
}

//---------------------------------------------------------------

void EtherCAT_clear_slaves(EtherCAT_master_t *master)
{
  unsigned int i;

  if (master->slave_count == 0) return;

  for (i = 0; i < master->slave_count; i++)
  {
    EtherCAT_slave_clear(&master->slaves[i]);
  }

  free(master->slaves);
  master->slaves = NULL;
}

//---------------------------------------------------------------

int EtherCAT_read_slave_information(EtherCAT_master_t *master,
                                    unsigned short int node_address,
                                    unsigned short int offset,
                                    unsigned int *target)
{
  EtherCAT_command_t *cmd;
  unsigned char data[10];
  unsigned int tries;

  // Initiate read operation

  data[0] = 0x00;
  data[1] = 0x01;
  data[2] = offset & 0xFF;
  data[3] = (offset & 0xFF00) >> 8;
  data[4] = 0x00;
  data[5] = 0x00;

  if ((cmd = EtherCAT_write(master, node_address, 0x502, 6, data)) == NULL)
  {
    fprintf(stderr, "ERROR: Could not allocate command!\n");
    return -2;
  }

  if (EtherCAT_send_receive(master) != 0)
  {
    EtherCAT_remove_command(master, cmd);
    fprintf(stderr, "ERROR: Could not write to slave!\n");
    return -3;
  }

  if (cmd->working_counter != 1)
  {
    EtherCAT_remove_command(master, cmd);
    fprintf(stderr, "ERROR: Command not processed by slave!\n");
    return -4;
  }

  EtherCAT_remove_command(master, cmd);

  // Get status of read operation

  tries = 0;
  while (tries < 10)
  {
    if ((cmd = EtherCAT_read(master, node_address, 0x502, 10)) == NULL)
    {
      fprintf(stderr, "ERROR: Could not allocate command!\n");
      return -2;
    }
    
    if (EtherCAT_send_receive(master) != 0)
    {
      EtherCAT_remove_command(master, cmd);

      fprintf(stderr, "ERROR: Could not read from slave!\n");
      return -3;
    }
    
    if (cmd->working_counter != 1)
    {
      EtherCAT_remove_command(master, cmd);

      fprintf(stderr, "ERROR: Command not processed by slave!\n");
      return -4;
    }

    if ((cmd->data[1] & 0x81) == 0)
    {
#if 0
      printf("SLI status data: %02X %02X Address: %02X %02X\n", cmd->data[0], cmd->data[1], cmd->data[2], cmd->data[3]);
      printf("Data: %02X %02X %02X %02X\n", cmd->data[6], cmd->data[7], cmd->data[8], cmd->data[9]);
#endif

      memcpy(target, cmd->data + 6, 4);

      EtherCAT_remove_command(master, cmd);

      break;
    }
    
    EtherCAT_remove_command(master, cmd);

    tries++;
  }

  if (tries == 10) fprintf(stderr, "ERROR: Timeout while reading SII!\n");

  return 0;
}

//---------------------------------------------------------------

int EtherCAT_send_receive(EtherCAT_master_t *master)
{
  libnet_ptag_t ptag;
  struct pcap_pkthdr header;
  const unsigned char *packet;
  unsigned char dst[6], src[6];
  unsigned int i, length, framelength, pos, command_type, command_index;
  EtherCAT_command_t *cmd;
  unsigned char *data;
  int bytes, command_follows, found;

#ifdef DEBUG_SEND_RECEIVE
  found = 0;
#endif

  length = 0;
  for (cmd = master->first_command; cmd != NULL; cmd = cmd->next)
  {
    //if (cmd->status != Waiting) continue;
    length += cmd->data_length + 12;

#ifdef DEBUG_SEND_RECEIVE
    found++;
#endif
  }

#ifdef DEBUG_SEND_RECEIVE
  printf("Sending %i commands with length %i...\n", found, length);
#endif

  if (length == 0) return 0;

  framelength = length + 2;
  if (framelength < 46) framelength = 46;

  data = (unsigned char *) malloc(sizeof(unsigned char) * framelength);
  if (!data) return -1;

  data[0] = length & 0xFF;
  data[1] = ((length & 0x700) >> 8) | 0x10;
  pos = 2;

  for (cmd = master->first_command; cmd != NULL; cmd = cmd->next)
  {
    if (cmd->status != Waiting)
    {
      printf("Old command: %02X\n", cmd->command_type);
      continue;
    }

    cmd->command_index = master->command_index;
    master->command_index = (master->command_index + 1) % 0x0100;

    cmd->status = Sent;

    data[pos + 0] = cmd->command_type;
    data[pos + 1] = cmd->command_index;

    switch (cmd->command_type)
    {
      case EC_CMD_APRD:
      case EC_CMD_APWR:
        data[pos + 2] = cmd->ring_position & 0xFF;
        data[pos + 3] = (cmd->ring_position & 0xFF00) >> 8;
        data[pos + 4] = cmd->mem_address & 0xFF;
        data[pos + 5] = (cmd->mem_address & 0xFF00) >> 8;
        break;

      case EC_CMD_NPRD:
      case EC_CMD_NPWR:
        data[pos + 2] = cmd->node_address & 0xFF;
        data[pos + 3] = (cmd->node_address & 0xFF00) >> 8;
        data[pos + 4] = cmd->mem_address & 0xFF;
        data[pos + 5] = (cmd->mem_address & 0xFF00) >> 8;
        break;

      case EC_CMD_BRD:
      case EC_CMD_BWR:
        data[pos + 2] = 0x00;
        data[pos + 3] = 0x00;
        data[pos + 4] = cmd->mem_address & 0xFF;
        data[pos + 5] = (cmd->mem_address & 0xFF00) >> 8;
        break;

      case EC_CMD_LRW:
        data[pos + 2] = cmd->logical_address & 0x000000FF;
        data[pos + 3] = (cmd->logical_address & 0x0000FF00) >> 8;
        data[pos + 4] = (cmd->logical_address & 0x00FF0000) >> 16;
        data[pos + 5] = (cmd->logical_address & 0xFF000000) >> 24;
        break;

      default:
        data[pos + 2] = 0x00;
        data[pos + 3] = 0x00;
        data[pos + 4] = 0x00;
        data[pos + 5] = 0x00;
        fprintf(stderr, "WARNING: Default adress while frame construction...\n");
        break;
    }

    data[pos + 6] = cmd->data_length & 0xFF;
    data[pos + 7] = (cmd->data_length & 0x700) >> 8;

    if (cmd->next) data[pos + 7] |= 0x80;

    data[pos + 8] = 0x00;
    data[pos + 9] = 0x00;

    if (cmd->command_type == EC_CMD_APWR
        || cmd->command_type == EC_CMD_NPWR
        || cmd->command_type == EC_CMD_BWR
        || cmd->command_type == EC_CMD_LRW) // Write
    {
      for (i = 0; i < cmd->data_length; i++) data[pos + 10 + i] = cmd->data[i];
    }
    else // Read
    {
      for (i = 0; i < cmd->data_length; i++) data[pos + 10 + i] = 0x00;
    }

    data[pos + 10 + cmd->data_length] = 0x00;
    data[pos + 11 + cmd->data_length] = 0x00;

    pos += 12 + cmd->data_length;
  }

  // Pad with zeros
  while (pos < 46) data[pos++] = 0x00;

#ifdef DEBUG_SEND_RECEIVE
  printf("\n>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
  for (i = 0; i < framelength; i++)
  {
    printf("%02X ", data[i]);

    if ((i + 1) % 16 == 0) printf("\n");
  }
  printf("\n-----------------------------------------------\n");
#endif
 
  dst[0] = 0xFF;
  dst[1] = 0xFF;
  dst[2] = 0xFF;
  dst[3] = 0xFF;
  dst[4] = 0xFF;
  dst[5] = 0xFF;

  src[0] = 0x00;
  src[1] = 0x00;
  src[2] = 0x00;
  src[3] = 0x00;
  src[4] = 0x00;
  src[5] = 0x00;

  // Send frame
  ptag = libnet_build_ethernet(dst, src, 0x88A4, data, framelength, master->net_handle, 0);
  bytes = libnet_write(master->net_handle);
  libnet_clear_packet(master->net_handle);

  if (bytes == -1)
  {
    free(data);
    fprintf(stderr, "Could not write!\n");
    return -1;
  }

  packet = pcap_next(master->pcap_handle, &header); // LibPCap receives sent frame first
  packet = pcap_next(master->pcap_handle, &header);

#ifdef DEBUG_SEND_RECEIVE
  for (i = 0; i < header.len - 14; i++)
  {
    if (packet[i + 14] == data[i]) printf("   ");
    else printf("%02X ", packet[i + 14]);

    if ((i + 1) % 16 == 0) printf("\n");
  }
  printf("\n<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n\n");
#endif

  // Free sent data
  free(data);

  pos = 16;
  command_follows = 1;
  while (command_follows)
  {
    command_type = packet[pos];
    command_index = packet[pos + 1];
    length = (packet[pos + 6] & 0xFF) | ((packet[pos + 7] & 0x07) << 8);
    command_follows = packet[pos + 7] & 0x80;

#if 0
    printf("Command %02X received!\n", command_index);
#endif

    found = 0;

    for (cmd = master->first_command; cmd != NULL; cmd = cmd->next)
    {
      if (cmd->status == Sent
          && cmd->command_type == command_type
          && cmd->command_index == command_index
          && cmd->data_length == length)
      {
        found = 1;
        cmd->status = Received;

        cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length);
        memcpy(cmd->data, packet + pos + 10, length);

        cmd->working_counter = (packet[pos + length + 10] & 0xFF)
          | ((packet[pos + length + 11] & 0xFF) << 8);
      }
    }

    if (!found)
    {
      fprintf(stderr, "WARNING: Command not assigned!\n");
    }

    pos += length + 12;
  }

  for (cmd = master->first_command; cmd != NULL; cmd = cmd->next)
  {
    if (cmd->status == Sent)
    {
      fprintf(stderr, "WARNING: Command not sent!\n");
    }
  }
      
  return 0;
}

//---------------------------------------------------------------

EtherCAT_command_t *EtherCAT_read(EtherCAT_master_t *master,
                                  unsigned short node_address,
                                  unsigned short offset,
                                  unsigned int length)
{
  EtherCAT_command_t *cmd;

  if (node_address == 0x0000) fprintf(stderr, "WARNING: Using node address 0x0000!\n");

  cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));

  if (cmd == NULL)
  {
    return NULL;
  }

  EtherCAT_command_init(cmd);

  cmd->command_type = EC_CMD_NPRD;
  cmd->node_address = node_address;
  cmd->mem_address = offset;
  cmd->data_length = length;

  // Add command to master's list
  add_command(master, cmd);

  return cmd;
}

//---------------------------------------------------------------

EtherCAT_command_t *EtherCAT_write(EtherCAT_master_t *master,
                                   unsigned short node_address,
                                   unsigned short offset,
                                   unsigned int length,
                                   const unsigned char *data)
{
  EtherCAT_command_t *cmd;

  if (node_address == 0x0000) fprintf(stderr, "WARNING: Using node address 0x0000!\n");

  cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));

  if (cmd == NULL)
  {
    return NULL;
  }

  EtherCAT_command_init(cmd);

  cmd->command_type = EC_CMD_NPWR;
  cmd->node_address = node_address;
  cmd->mem_address = offset;
  cmd->data_length = length;

  cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length);

  if (cmd->data == NULL)
  {
    free(cmd);
    return NULL;
  }

  memcpy(cmd->data, data, length);

  // Add command to master's list
  add_command(master, cmd);

  return cmd;
}

//---------------------------------------------------------------

EtherCAT_command_t *EtherCAT_position_read(EtherCAT_master_t *master,
                                           short ring_position,
                                           unsigned short offset,
                                           unsigned int length)
{
  EtherCAT_command_t *cmd;

  cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));

  if (cmd == NULL)
  {
    return NULL;
  }

  EtherCAT_command_init(cmd);

  cmd->command_type = EC_CMD_APRD;
  cmd->ring_position = ring_position;
  cmd->mem_address = offset;
  cmd->data_length = length;

  // Add command to master's list
  add_command(master, cmd);

  return cmd;
}

//---------------------------------------------------------------

EtherCAT_command_t *EtherCAT_position_write(EtherCAT_master_t *master,
                                            short ring_position,
                                            unsigned short offset,
                                            unsigned int length,
                                            const unsigned char *data)
{
  EtherCAT_command_t *cmd;

  cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));

  if (cmd == NULL)
  {
    return NULL;
  }

  EtherCAT_command_init(cmd);

  cmd->command_type = EC_CMD_APWR;
  cmd->ring_position = ring_position;
  cmd->mem_address = offset;
  cmd->data_length = length;

  cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length);

  if (cmd->data == NULL)
  {
    free(cmd);
    return NULL;
  }

  memcpy(cmd->data, data, length);

  // Add command to master's list
  add_command(master, cmd);

  return cmd;
}

//---------------------------------------------------------------

EtherCAT_command_t *EtherCAT_broadcast_read(EtherCAT_master_t *master,
                                            unsigned short offset,
                                            unsigned int length)
{
  EtherCAT_command_t *cmd;

  cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));

  if (cmd == NULL)
  {
    return NULL;
  }

  EtherCAT_command_init(cmd);

  cmd->command_type = EC_CMD_BRD;
  cmd->mem_address = offset;
  cmd->data_length = length;

  // Add command to master's list
  add_command(master, cmd);

  return cmd;
}

//---------------------------------------------------------------

EtherCAT_command_t *EtherCAT_broadcast_write(EtherCAT_master_t *master,
                                             unsigned short offset,
                                             unsigned int length,
                                             const unsigned char *data)
{
  EtherCAT_command_t *cmd;

  cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));

  if (cmd == NULL)
  {
    return NULL;
  }

  EtherCAT_command_init(cmd);

  cmd->command_type = EC_CMD_BWR;
  cmd->mem_address = offset;
  cmd->data_length = length;

  cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length);

  if (cmd->data == NULL)
  {
    free(cmd);
    return NULL;
  }

  memcpy(cmd->data, data, length);

  // Add command to master's list
  add_command(master, cmd);

  return cmd;
}

//---------------------------------------------------------------

EtherCAT_command_t *EtherCAT_logical_read_write(EtherCAT_master_t *master,
                                                unsigned int offset,
                                                unsigned int length,
                                                unsigned char *data)
{
  EtherCAT_command_t *cmd;

  cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));

  if (cmd == NULL)
  {
    return NULL;
  }

  EtherCAT_command_init(cmd);

  cmd->command_type = EC_CMD_LRW;
  cmd->mem_address = offset;
  cmd->data_length = length;

  cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length);

  if (cmd->data == NULL)
  {
    free(cmd);
    return NULL;
  }

  memcpy(cmd->data, data, length);

  // Add command to master's list
  add_command(master, cmd);

  return cmd;
}

//---------------------------------------------------------------

void add_command(EtherCAT_master_t *master, EtherCAT_command_t *cmd)
{
  EtherCAT_command_t **last_cmd;

  // Find last position in the list
  last_cmd = &(master->first_command);
  while (*last_cmd) last_cmd = &(*last_cmd)->next;

  *last_cmd = cmd;
}

//---------------------------------------------------------------

void EtherCAT_remove_command(EtherCAT_master_t *master,
                             EtherCAT_command_t *rem_cmd)
{
  EtherCAT_command_t **last_cmd;

  last_cmd = &(master->first_command);
  while (*last_cmd)
  {
    if (*last_cmd == rem_cmd)
    {
      *last_cmd = rem_cmd->next;
      EtherCAT_command_clear(rem_cmd);

      return;
    }

    last_cmd = &(*last_cmd)->next;
  }
}

//---------------------------------------------------------------

int EtherCAT_state_change(EtherCAT_master_t *master,
                          EtherCAT_slave_t *slave,
                          unsigned char state_and_ack)
{
  EtherCAT_command_t *cmd;
  unsigned char data[2];
  unsigned int tries_left;

  data[0] = state_and_ack;
  data[1] = 0x00;

  if ((cmd = EtherCAT_write(master, slave->station_address,
                            0x0120, 2, data)) == NULL)
  {
    return -1;
  }

  if (EtherCAT_send_receive(master) != 0)
  {
    EtherCAT_remove_command(master, cmd);
    return -2;
  }

  if (cmd->working_counter != 1)
  {
    EtherCAT_remove_command(master, cmd);
    return -3;
  }

  EtherCAT_remove_command(master, cmd);

  slave->requested_state = state_and_ack & 0x0F;

  tries_left = 10;

  while (tries_left)
  {
    if ((cmd = EtherCAT_read(master, slave->station_address,
                             0x0130, 2)) == NULL)
    {
      return -1;
    }
    
    if (EtherCAT_send_receive(master) != 0)
    {
      EtherCAT_remove_command(master, cmd);
      return -2;
    }
    
    if (cmd->working_counter != 1)
    {
      EtherCAT_remove_command(master, cmd);
      return -3;
    }

    if (cmd->data[0] & 0x10) // State change error
    {
      EtherCAT_remove_command(master, cmd);
      return -4;
    }

    if (cmd->data[0] == state_and_ack & 0x0F) // State change successful
    {
      EtherCAT_remove_command(master, cmd);
      break;
    }

    EtherCAT_remove_command(master, cmd);

    //printf("Trying again...\n");

    tries_left--;
  }

  if (!tries_left)
  {
    return -5;
  }

  slave->current_state = state_and_ack & 0x0F;

  return 0;
}

//---------------------------------------------------------------

int EtherCAT_broadcast_state_change(EtherCAT_master_t *master,
                                    unsigned char state)
{
  EtherCAT_command_t *cmd;
  unsigned char data[2];
  unsigned int tries_left;

  data[0] = state;
  data[1] = 0x00;

  if ((cmd = EtherCAT_broadcast_write(master, 0x0120, 2, data)) == NULL)
  {
    return -1;
  }

  if (EtherCAT_send_receive(master) != 0)
  {
    EtherCAT_remove_command(master, cmd);
    return -2;
  }

  if (cmd->working_counter != master->slave_count)
  {
    EtherCAT_remove_command(master, cmd);
    return -3;
  }

  EtherCAT_remove_command(master, cmd);

  tries_left = 10;

  while (tries_left)
  {
    if ((cmd = EtherCAT_broadcast_read(master, 0x0130, 2)) == NULL)
    {
      return -1;
    }
    
    if (EtherCAT_send_receive(master) != 0)
    {
      EtherCAT_remove_command(master, cmd);
      return -2;
    }
    
    if (cmd->working_counter != master->slave_count)
    {
      EtherCAT_remove_command(master, cmd);
      return -3;
    }

    if (cmd->data[0] & 0x10) // State change error
    {
      EtherCAT_remove_command(master, cmd);
      return -4;
    }

    if (cmd->data[0] == state) // State change successful
    {
      EtherCAT_remove_command(master, cmd);
      break;
    }

    EtherCAT_remove_command(master, cmd);

    //printf("Trying again...\n");

    tries_left--;
  }

  if (!tries_left)
  {
    return -5;
  }

  return 0;
}

//---------------------------------------------------------------

int EtherCAT_start(EtherCAT_master_t *master,
                   unsigned int length,
                   void (*pre_cb)(unsigned char *),
                   void (*post_cb)(unsigned char *),
                   unsigned int cycle_time)
{
  if (master->process_data)
  {
    fprintf(stderr, "ERROR: Process data already allocated!\n");
    return -1;
  }

  if ((master->process_data = (unsigned char *) malloc(length)) == NULL)
  {
    fprintf(stderr, "ERROR: Could not allocate process data block!\n");
    return -2;
  }

  memset(master->process_data, 0x00, length);

  master->process_data_length = length;
  master->pre_cb = pre_cb;
  master->post_cb = post_cb;
  master->cycle_time = cycle_time;

  master->thread_continue = 1;

  if (pthread_create(&master->thread, NULL, thread_function, (void *) master) != 0)
  {
    fprintf(stderr, "ERROR: Could not create thread!\n");
    return -3;
  }

  return 0;
}

//---------------------------------------------------------------

int EtherCAT_stop(EtherCAT_master_t *master)
{
  if (!master->thread_continue)
  {
    fprintf(stderr, "ERROR: Thread not running!\n");
    return -1;
  }

  master->thread_continue = 0;
  pthread_join(master->thread, NULL);

  if (master->process_data)
  {
    free(master->process_data);
    master->process_data = NULL;
  }

  master->pre_cb = NULL;
  master->post_cb = NULL;

  return 0;
}

//---------------------------------------------------------------

double current_timestamp()
{
  struct timeval tv;
  gettimeofday(&tv, NULL);
  return tv.tv_sec + (double) tv.tv_usec / 1000000.0;
}

//---------------------------------------------------------------

void *thread_function(void *data)
{
  EtherCAT_master_t *master;
  EtherCAT_command_t *cmd;
  double bus_start_time, bus_end_time;
  double cycle_start_time, cycle_end_time, last_cycle_start_time;
  unsigned int wait_usecs;

  master = (EtherCAT_master_t *) data;

  last_cycle_start_time = 0.0;

  while (master->thread_continue)
  {
    cycle_start_time = current_timestamp();

    if (last_cycle_start_time != 0.0)
    {
      master->last_cycle_time = cycle_start_time - last_cycle_start_time;
      master->last_jitter = (master->last_cycle_time - (master->cycle_time / 1000000.0))
        / (master->cycle_time / 1000000.0) * 100.0;
    }

    last_cycle_start_time = cycle_start_time;

    if (master->pre_cb) master->pre_cb(master->process_data);

    cmd = EtherCAT_logical_read_write(master,
                                      0x00000000,
                                      master->process_data_length,
                                      master->process_data);

    bus_start_time = current_timestamp();

    EtherCAT_send_receive(master);

    bus_end_time = current_timestamp();
    master->bus_time = bus_end_time - bus_start_time;
    
#if 0
    printf("Working counter: %i\n", cmd->working_counter);
#endif
    
    memcpy(master->process_data, cmd->data, master->process_data_length);

    EtherCAT_remove_command(master, cmd);

    if (master->post_cb) master->post_cb(master->process_data);

    // Calculate working time

    cycle_end_time = current_timestamp();
    master->last_cycle_work_time = cycle_end_time - cycle_start_time;
    master->last_cycle_busy_rate = master->last_cycle_work_time
      / ((double) master->cycle_time / 1000000.0) * 100.0;
    wait_usecs = master->cycle_time - (unsigned int) (master->last_cycle_work_time * 1000000.0);

    //printf("USECS to wait: %i\n", wait_usecs);

    usleep(wait_usecs);

    //printf("waited: %lf\n", current_timestamp() - cycle_end_time);
  }

  return (void *) 0;
}

//---------------------------------------------------------------

int EtherCAT_activate_slave(EtherCAT_master_t *master,
                            EtherCAT_slave_t *slave)
{
  EtherCAT_command_t *cmd;
  const EtherCAT_slave_desc_t *desc;
  unsigned char fmmu[16];
  unsigned char data[256];

  if (EtherCAT_state_change(master, slave, EC_STATE_INIT) != 0)
  {
    return -1;
  }

  // Resetting FMMU's

  memset(data, 0x00, 256);
  cmd = EtherCAT_write(master, slave->station_address, 0x0600, 256, data);
  EtherCAT_send_receive(master);

  if (cmd->working_counter != 1)
  {
    fprintf(stderr, "ERROR: Slave did not respond!\n");
    return -2;
  }

  EtherCAT_remove_command(master, cmd);

  // Resetting Sync Manager channels

  memset(data, 0x00, 256);
  cmd = EtherCAT_write(master, slave->station_address, 0x0800, 256, data);
  EtherCAT_send_receive(master);

  if (cmd->working_counter != 1)
  {
    fprintf(stderr, "ERROR: Slave did not respond!\n");
    return -2;
  }

  EtherCAT_remove_command(master, cmd);

  desc = slave->desc;

  // Init Mailbox communication

  if (desc->type == MAILBOX)
  {
    if (desc->sm0)
    {
      cmd = EtherCAT_write(master, slave->station_address, 0x0800, 8, desc->sm0);
      
      EtherCAT_send_receive(master);
      
      if (cmd->working_counter != 1)
      {
        fprintf(stderr, "ERROR: Not all slaves responded!\n");
        return -3;
      }
      
      EtherCAT_remove_command(master, cmd);
    }

    if (desc->sm1)
    {
      cmd = EtherCAT_write(master, slave->station_address, 0x0808, 8, desc->sm1);
      
      EtherCAT_send_receive(master);
      
      if (cmd->working_counter != 1)
      {
        fprintf(stderr, "ERROR: Not all slaves responded!\n");
        return -4;
      }
      
      EtherCAT_remove_command(master, cmd);
    }
  }

  // Change state to PREOP

  if (EtherCAT_state_change(master, slave, EC_STATE_PREOP) != 0)
  {
    fprintf(stderr, "ERROR: Could not set state!\n");
    return -5;
  }

  // Set FMMU's

  if (desc->fmmu0)
  {
    memcpy(fmmu, desc->fmmu0, 16);

    fmmu[0] = slave->logical_address0 & 0x000000FF;
    fmmu[1] = (slave->logical_address0 & 0x0000FF00) >> 8;    
    fmmu[2] = (slave->logical_address0 & 0x00FF0000) >> 16;
    fmmu[3] = (slave->logical_address0 & 0xFF000000) >> 24;

    cmd = EtherCAT_write(master, slave->station_address, 0x0600, 16, fmmu);
    EtherCAT_send_receive(master);

    if (cmd->working_counter != 1)
    {
      fprintf(stderr, "ERROR: Not all slaves responded (%i of 1)!\n",
              cmd->working_counter);
      return -6;
    }

    EtherCAT_remove_command(master, cmd);
  }

  // Set Sync Managers

  if (desc->type != MAILBOX)
  {
    if (desc->sm0)
    {
      cmd = EtherCAT_write(master, slave->station_address, 0x0800, 8, desc->sm0);
      
      EtherCAT_send_receive(master);
      
      if (cmd->working_counter != 1)
      {
        fprintf(stderr, "ERROR: Not all slaves responded!\n");
        return -8;
      }
      
      EtherCAT_remove_command(master, cmd);
    }

    if (desc->sm1)
    {
      cmd = EtherCAT_write(master, slave->station_address, 0x0808, 8, desc->sm1);
      
      EtherCAT_send_receive(master);
      
      if (cmd->working_counter != 1)
      {
        fprintf(stderr, "ERROR: Not all slaves responded!\n");
        return -9;
      }
      
      EtherCAT_remove_command(master, cmd);
    }
  }
  
  if (desc->sm2)
  {
    cmd = EtherCAT_write(master, slave->station_address, 0x0810, 8, desc->sm2);
      
    EtherCAT_send_receive(master);
      
    if (cmd->working_counter != 1)
    {
      fprintf(stderr, "ERROR: Not all slaves responded!\n");
      return -10;
    }
      
    EtherCAT_remove_command(master, cmd);
  }

  if (desc->sm3)
  {
    cmd = EtherCAT_write(master, slave->station_address, 0x0818, 8, desc->sm3);
      
    EtherCAT_send_receive(master);
      
    if (cmd->working_counter != 1)
    {
      fprintf(stderr, "ERROR: Not all slaves responded!\n");
      return -11;
    }
      
    EtherCAT_remove_command(master, cmd);
  }

  // Change state to SAVEOP

  if (EtherCAT_state_change(master, slave, EC_STATE_SAVEOP) != 0)
  {
    fprintf(stderr, "ERROR: Could not set state!\n");
    return -12;
  }

  // Change state to OP

  if (EtherCAT_state_change(master, slave, EC_STATE_OP) != 0)
  {
    fprintf(stderr, "ERROR: Could not set state!\n");
    return -13;
  }

  return 0;
}

//---------------------------------------------------------------

int EtherCAT_deactivate_slave(EtherCAT_master_t *master,
                            EtherCAT_slave_t *slave)
{
  if (EtherCAT_state_change(master, slave, EC_STATE_INIT) != 0)
  {
    return -1;
  }

  return 0;
}

//---------------------------------------------------------------

void set_byte(unsigned char *data,
              unsigned int offset,
              unsigned char value)
{
  data[offset] = value;
}

//---------------------------------------------------------------

void set_word(unsigned char *data,
              unsigned int offset,
              unsigned int value)
{
  data[offset] = value & 0xFF;
  data[offset + 1] = (value & 0xFF00) >> 8;
}

//---------------------------------------------------------------