1

/******************************************************************************

     2

 *  

     3

 *  $Id$

     4

 *  

     5

 *  Copyright (C) 2006-2009  Florian Pose, Ingenieurgemeinschaft IgH

     6

 *  

     7

 *  This file is part of the IgH EtherCAT master userspace library.

     8

 *  

     9

 *  The IgH EtherCAT master userspace library is free software; you can

    10

 *  redistribute it and/or modify it under the terms of the GNU Lesser General

    11

 *  Public License as published by the Free Software Foundation; version 2.1

    12

 *  of the License.

    13

 *

    14

 *  The IgH EtherCAT master userspace library is distributed in the hope that

    15

 *  it will be useful, but WITHOUT ANY WARRANTY; without even the implied

    16

 *  warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    17

 *  GNU Lesser General Public License for more details.

    18

 *

    19

 *  You should have received a copy of the GNU Lesser General Public License

    20

 *  along with the IgH EtherCAT master userspace library. If not, see

    21

 *  <http://www.gnu.org/licenses/>.

    22

 *  

    23

 *  ---

    24

 *  

    25

 *  The license mentioned above concerns the source code only. Using the

    26

 *  EtherCAT technology and brand is only permitted in compliance with the

    27

 *  industrial property and similar rights of Beckhoff Automation GmbH.

    28

 *

    29

 *****************************************************************************/

    30

    31

#include <stdlib.h>

    32

#include <sys/ioctl.h>

    33

#include <stdio.h>

    34

#include <errno.h>

    35

#include <string.h>

    36

#include <sys/mman.h>

    37

    38

#include "master.h"

    39

#include "domain.h"

    40

#include "slave_config.h"

    41

#include "master/ioctl.h"

    42

    43

/*****************************************************************************/

    44

    45

int ecrt_master_reserve(ec_master_t *master)

    46

{

    47

    if (ioctl(master->fd, EC_IOCTL_REQUEST, NULL) == -1) {

    48

        fprintf(stderr, "Failed to reserve master: %s\n",

    49

                strerror(errno));

    50

        return -1;

    51

    }

    52

}

    53

    54

/*****************************************************************************/

    55

    56

void ec_master_clear_config(ec_master_t *master)

    57

{

    58

    ec_domain_t *d, *next_d;

    59

    ec_slave_config_t *c, *next_c;

    60

    61

    d = master->first_domain;

    62

    while (d) {

    63

        next_d = d->next;

    64

        ec_domain_clear(d);

    65

        d = next_d;

    66

    }

    67

    master->first_domain = NULL;

    68

    69

    c = master->first_config;

    70

    while (c) {

    71

        next_c = c->next;

    72

        ec_slave_config_clear(c);

    73

        c = next_c;

    74

    }

    75

    master->first_config = NULL;

    76

}

    77

    78

/*****************************************************************************/

    79

    80

void ec_master_clear(ec_master_t *master)

    81

{

    82

    if (master->process_data)  {

    83

        munmap(master->process_data, master->process_data_size);

    84

    }

    85

    86

    ec_master_clear_config(master);

    87

    88

    if (master->fd != -1) {

    89

        close(master->fd);

    90

    }

    91

}

    92

    93

/*****************************************************************************/

    94

    95

void ec_master_add_domain(ec_master_t *master, ec_domain_t *domain)

    96

{

    97

    if (master->first_domain) {

    98

        ec_domain_t *d = master->first_domain;

    99

        while (d->next) {

   100

            d = d->next;

   101

        }

   102

        d->next = domain;

   103

    } else {

   104

        master->first_domain = domain;

   105

    }

   106

}

   107

   108

/*****************************************************************************/

   109

   110

ec_domain_t *ecrt_master_create_domain(ec_master_t *master)

   111

{

   112

    ec_domain_t *domain;

   113

    int index;

   114

   115

    domain = malloc(sizeof(ec_domain_t));

   116

    if (!domain) {

   117

        fprintf(stderr, "Failed to allocate memory.\n");

   118

        return 0;

   119

    }

   120

   121

    index = ioctl(master->fd, EC_IOCTL_CREATE_DOMAIN, NULL);

   122

    if (index == -1) {

   123

        fprintf(stderr, "Failed to create domain: %s\n", strerror(errno));

   124

        free(domain);

   125

        return 0; 

   126

    }

   127

   128

    domain->next = NULL;

   129

    domain->index = (unsigned int) index;

   130

    domain->master = master;

   131

    domain->process_data = NULL;

   132

   133

    ec_master_add_domain(master, domain);

   134

   135

    return domain;

   136

}

   137

   138

/*****************************************************************************/

   139

   140

void ec_master_add_slave_config(ec_master_t *master, ec_slave_config_t *sc)

   141

{

   142

    if (master->first_config) {

   143

        ec_slave_config_t *c = master->first_config;

   144

        while (c->next) {

   145

            c = c->next;

   146

        }

   147

        c->next = sc;

   148

    } else {

   149

        master->first_config = sc;

   150

    }

   151

}

   152

   153

/*****************************************************************************/

   154

   155

ec_slave_config_t *ecrt_master_slave_config(ec_master_t *master,

   156

        uint16_t alias, uint16_t position, uint32_t vendor_id,

   157

        uint32_t product_code)

   158

{

   159

    ec_ioctl_config_t data;

   160

    ec_slave_config_t *sc;

   161

    int index;

   162

   163

    sc = malloc(sizeof(ec_slave_config_t));

   164

    if (!sc) {

   165

        fprintf(stderr, "Failed to allocate memory.\n");

   166

        return 0;

   167

    }

   168

   169

    data.alias = alias;

   170

    data.position = position;

   171

    data.vendor_id = vendor_id;

   172

    data.product_code = product_code;

   173

   174

    if (ioctl(master->fd, EC_IOCTL_CREATE_SLAVE_CONFIG, &data) == -1) {

   175

        fprintf(stderr, "Failed to create slave config: %s\n",

   176

                strerror(errno));

   177

        free(sc);

   178

        return 0; 

   179

    }

   180

   181

    sc->next = NULL;

   182

    sc->master = master;

   183

    sc->index = data.config_index;

   184

    sc->alias = alias;

   185

    sc->position = position;

   186

    sc->first_sdo_request = NULL;

   187

    sc->first_voe_handler = NULL;

   188

   189

    ec_master_add_slave_config(master, sc);

   190

   191

    return sc;

   192

}

   193

   194

/*****************************************************************************/

   195

   196

int ecrt_master(ec_master_t* master, ec_master_info_t *master_info)

   197

{

   198

    ec_ioctl_master_t data;

   199

   200

    if (ioctl(master->fd, EC_IOCTL_MASTER, &data) < 0) {

   201

        fprintf(stderr, "Failed to get master info: %s\n", strerror(errno));

   202

        return -1;

   203

    }

   204

   205

    master_info->slave_count = data.slave_count;

   206

    master_info->link_up = data.devices[0].link_state;

   207

    master_info->scan_busy = data.scan_busy;

   208

    master_info->app_time = data.app_time;

   209

    return 0;

   210

}

   211

   212

/*****************************************************************************/

   213

   214

int ecrt_master_get_slave(ec_master_t *master, uint16_t slave_position,

   215

        ec_slave_info_t *slave_info)

   216

{

   217

    ec_ioctl_slave_t data;

   218

    int index;

   219

   220

    data.position = slave_position;

   221

   222

    if (ioctl(master->fd, EC_IOCTL_SLAVE, &data) == -1) {

   223

        fprintf(stderr, "Failed to get slave info: %s\n", strerror(errno));

   224

        return -1;

   225

    }

   226

   227

    slave_info->position = data.position;

   228

    slave_info->vendor_id = data.vendor_id;

   229

    slave_info->product_code = data.product_code;

   230

    slave_info->revision_number = data.revision_number;

   231

    slave_info->serial_number = data.serial_number;

   232

    slave_info->alias = data.alias;

   233

    slave_info->current_on_ebus = data.current_on_ebus;

   234

    slave_info->al_state = data.al_state;

   235

    slave_info->error_flag = data.error_flag;

   236

    slave_info->sync_count = data.sync_count;

   237

    slave_info->sdo_count = data.sdo_count;

   238

    strncpy(slave_info->name, data.name, EC_MAX_STRING_LENGTH);

   239

    return 0;

   240

}

   241

   242

/*****************************************************************************/

   243

   244

int ecrt_master_get_sync_manager(ec_master_t *master, uint16_t slave_position,

   245

        uint8_t sync_index, ec_sync_info_t *sync)

   246

{

   247

    ec_ioctl_slave_sync_t data;

   248

   249

    if (sync_index >= EC_MAX_SYNC_MANAGERS)

   250

        return -ENOENT;

   251

   252

    memset(&data, 0x00, sizeof(ec_ioctl_slave_sync_t));

   253

    data.slave_position = slave_position;

   254

    data.sync_index = sync_index;

   255

   256

    if (ioctl(master->fd, EC_IOCTL_SLAVE_SYNC, &data) == -1) {

   257

        fprintf(stderr, "Failed to get sync manager information: %s\n",

   258

                strerror(errno));

   259

        return -1; // FIXME

   260

    }

   261

   262

    sync->index = sync_index;

   263

    sync->dir = EC_READ_BIT(&data.control_register, 2) ?

   264

        EC_DIR_OUTPUT : EC_DIR_INPUT;

   265

    sync->n_pdos = data.pdo_count;

   266

    sync->pdos = NULL;

   267

    sync->watchdog_mode = EC_READ_BIT(&data.control_register, 6) ?

   268

        EC_WD_ENABLE : EC_WD_DISABLE;

   269

   270

    return 0;

   271

}

   272

   273

/*****************************************************************************/

   274

   275

int ecrt_master_get_pdo(ec_master_t *master, uint16_t slave_position,

   276

        uint8_t sync_index, uint16_t pos, ec_pdo_info_t *pdo)

   277

{

   278

    ec_ioctl_slave_sync_pdo_t data;

   279

   280

    if (sync_index >= EC_MAX_SYNC_MANAGERS)

   281

        return -ENOENT;

   282

   283

    memset(&data, 0x00, sizeof(ec_ioctl_slave_sync_pdo_t));

   284

    data.slave_position = slave_position;

   285

    data.sync_index = sync_index;

   286

    data.pdo_pos = pos;

   287

   288

    if (ioctl(master->fd, EC_IOCTL_SLAVE_SYNC_PDO, &data) == -1) {

   289

        fprintf(stderr, "Failed to get pdo information: %s\n",

   290

                strerror(errno));

   291

        return -1; // FIXME

   292

    }

   293

   294

    pdo->index = data.index;

   295

    pdo->n_entries = data.entry_count;

   296

    pdo->entries = NULL;

   297

   298

    return 0;

   299

}

   300

   301

/*****************************************************************************/

   302

   303

int ecrt_master_get_pdo_entry(ec_master_t *master, uint16_t slave_position,

   304

        uint8_t sync_index, uint16_t pdo_pos, uint16_t entry_pos,

   305

        ec_pdo_entry_info_t *entry)

   306

{

   307

    ec_ioctl_slave_sync_pdo_entry_t data;

   308

   309

    if (sync_index >= EC_MAX_SYNC_MANAGERS)

   310

        return -ENOENT;

   311

   312

    memset(&data, 0x00, sizeof(ec_ioctl_slave_sync_pdo_entry_t));

   313

    data.slave_position = slave_position;

   314

    data.sync_index = sync_index;

   315

    data.pdo_pos = pdo_pos;

   316

    data.entry_pos = entry_pos;

   317

   318

    if (ioctl(master->fd, EC_IOCTL_SLAVE_SYNC_PDO_ENTRY, &data) == -1) {

   319

        fprintf(stderr, "Failed to get pdo entry information: %s\n",

   320

                strerror(errno));

   321

        return -1; // FIXME

   322

    }

   323

   324

    entry->index = data.index;

   325

    entry->subindex = data.subindex;

   326

    entry->bit_length = data.bit_length;

   327

   328

    return 0;

   329

}

   330

   331

/*****************************************************************************/

   332

   333

int ecrt_master_sdo_download(ec_master_t *master, uint16_t slave_position,

   334

        uint16_t index, uint8_t subindex, uint8_t *data,

   335

        size_t data_size, uint32_t *abort_code)

   336

{

   337

    ec_ioctl_slave_sdo_download_t download;

   338

   339

    download.slave_position = slave_position;

   340

    download.sdo_index = index;

   341

    download.sdo_entry_subindex = subindex;

   342

    download.data_size = data_size;

   343

    download.data = data;

   344

   345

    if (ioctl(master->fd, EC_IOCTL_SLAVE_SDO_DOWNLOAD, &download) == -1) {

   346

        if (errno == EIO && abort_code) {

   347

            *abort_code = download.abort_code;

   348

        }

   349

        fprintf(stderr, "Failed to execute SDO download: %s\n",

   350

            strerror(errno));

   351

        return -1;

   352

    }

   353

   354

    return 0;

   355

}

   356

   357

/*****************************************************************************/

   358

   359

int ecrt_master_sdo_upload(ec_master_t *master, uint16_t slave_position,

   360

        uint16_t index, uint8_t subindex, uint8_t *target,

   361

        size_t target_size, size_t *result_size, uint32_t *abort_code)

   362

{

   363

    ec_ioctl_slave_sdo_upload_t upload;

   364

   365

    upload.slave_position = slave_position;

   366

    upload.sdo_index = index;

   367

    upload.sdo_entry_subindex = subindex;

   368

    upload.target_size = target_size;

   369

    upload.target = target;

   370

   371

    if (ioctl(master->fd, EC_IOCTL_SLAVE_SDO_UPLOAD, &upload) == -1) {

   372

        if (errno == EIO && abort_code) {

   373

            *abort_code = upload.abort_code;

   374

        }

   375

        fprintf(stderr, "Failed to execute SDO upload: %s\n",

   376

                strerror(errno));

   377

        return -1;

   378

    }

   379

   380

    *result_size = upload.data_size;

   381

    return 0;

   382

}

   383

   384

/*****************************************************************************/

   385

   386

int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,

   387

        uint8_t drive_no, uint16_t idn, uint8_t *data, size_t data_size,

   388

        uint16_t *error_code)

   389

{

   390

    ec_ioctl_slave_soe_write_t io;

   391

   392

    io.slave_position = slave_position;

   393

    io.drive_no = drive_no;

   394

    io.idn = idn;

   395

    io.data_size = data_size;

   396

    io.data = data;

   397

   398

    if (ioctl(master->fd, EC_IOCTL_SLAVE_SOE_WRITE, &io) == -1) {

   399

        if (errno == EIO && error_code) {

   400

            *error_code = io.error_code;

   401

        }

   402

        fprintf(stderr, "Failed to write IDN: %s\n", strerror(errno));

   403

        return -1;

   404

    }

   405

   406

    return 0;

   407

}

   408

   409

/*****************************************************************************/

   410

   411

int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position,

   412

        uint8_t drive_no, uint16_t idn, uint8_t *target, size_t target_size,

   413

        size_t *result_size, uint16_t *error_code)

   414

{

   415

    ec_ioctl_slave_soe_read_t io;

   416

   417

    io.slave_position = slave_position;

   418

    io.drive_no = drive_no;

   419

    io.idn = idn;

   420

    io.mem_size = target_size;

   421

    io.data = target;

   422

   423

    if (ioctl(master->fd, EC_IOCTL_SLAVE_SOE_READ, &io) == -1) {

   424

        if (errno == EIO && error_code) {

   425

            *error_code = io.error_code;

   426

        }

   427

        fprintf(stderr, "Failed to read IDN: %s\n", strerror(errno));

   428

        return -1;

   429

    }

   430

   431

    *result_size = io.data_size;

   432

    return 0;

   433

}

   434

   435

/*****************************************************************************/

   436

   437

int ecrt_master_activate(ec_master_t *master)

   438

{

   439

    if (ioctl(master->fd, EC_IOCTL_ACTIVATE,

   440

                &master->process_data_size) == -1) {

   441

        fprintf(stderr, "Failed to activate master: %s\n",

   442

                strerror(errno));

   443

        return -1; // FIXME

   444

    }

   445

   446

    if (master->process_data_size) {

   447

        master->process_data = mmap(0, master->process_data_size,

   448

                PROT_READ | PROT_WRITE, MAP_SHARED, master->fd, 0);

   449

        if (master->process_data == MAP_FAILED) {

   450

            fprintf(stderr, "Failed to map process data: %s", strerror(errno));

   451

            master->process_data = NULL;

   452

            master->process_data_size = 0;

   453

            return -1; // FIXME

   454

        }

   455

   456

        // Access the mapped region to cause the initial page fault

   457

        printf("pd: %x\n", master->process_data[0]);

   458

    }

   459

   460

    return 0;

   461

}

   462

   463

/*****************************************************************************/

   464

   465

void ecrt_master_deactivate(ec_master_t *master)

   466

{

   467

    if (ioctl(master->fd, EC_IOCTL_DEACTIVATE, NULL) == -1) {

   468

        fprintf(stderr, "Failed to deactivate master: %s\n", strerror(errno));

   469

        return;

   470

    }

   471

   472

    ec_master_clear_config(master);

   473

}

   474

   475

/*****************************************************************************/

   476

   477

int ecrt_master_set_send_interval(ec_master_t *master,size_t send_interval_us)

   478

{

   479

    if (ioctl(master->fd, EC_IOCTL_SET_SEND_INTERVAL,

   480

                &send_interval_us) == -1) {

   481

        fprintf(stderr, "Failed to set send interval: %s\n",

   482

                strerror(errno));

   483

        return -1; // FIXME

   484

    }

   485

    return 0;

   486

}

   487

   488

/*****************************************************************************/

   489

   490

void ecrt_master_send(ec_master_t *master)

   491

{

   492

    if (ioctl(master->fd, EC_IOCTL_SEND, NULL) == -1) {

   493

        fprintf(stderr, "Failed to send: %s\n", strerror(errno));

   494

    }