1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006-2012  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 <stdio.h>

    33

#include <string.h>

    34

#include <errno.h> /* ENOENT */

    35

    36

#include "ioctl.h"

    37

#include "slave_config.h"

    38

#include "domain.h"

    39

#include "sdo_request.h"

    40

#include "reg_request.h"

    41

#include "voe_handler.h"

    42

#include "master.h"

    43

    44

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

    45

    46

void ec_slave_config_clear(ec_slave_config_t *sc)

    47

{

    48

    ec_sdo_request_t *r, *next_r;

    49

    ec_reg_request_t *e, *next_e;

    50

    ec_voe_handler_t *v, *next_v;

    51

    52

    r = sc->first_sdo_request;

    53

    while (r) {

    54

        next_r = r->next;

    55

        ec_sdo_request_clear(r);

    56

        r = next_r;

    57

    }

    58

    59

    e = sc->first_reg_request;

    60

    while (e) {

    61

        next_e = e->next;

    62

        ec_reg_request_clear(e);

    63

        e = next_e;

    64

    }

    65

    66

    v = sc->first_voe_handler;

    67

    while (v) {

    68

        next_v = v->next;

    69

        ec_voe_handler_clear(v);

    70

        v = next_v;

    71

    }

    72

}

    73

    74

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

    75

    76

int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index,

    77

        ec_direction_t dir, ec_watchdog_mode_t watchdog_mode)

    78

{

    79

    ec_ioctl_config_t data;

    80

    int ret;

    81

    82

    if (sync_index >= EC_MAX_SYNC_MANAGERS)

    83

        return -ENOENT;

    84

    85

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

    86

    data.config_index = sc->index;

    87

    data.syncs[sync_index].dir = dir;

    88

    data.syncs[sync_index].watchdog_mode = watchdog_mode;

    89

    data.syncs[sync_index].config_this = 1;

    90

    91

    ret = ioctl(sc->master->fd, EC_IOCTL_SC_SYNC, &data);

    92

    if (EC_IOCTL_IS_ERROR(ret)) {

    93

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

    94

                strerror(EC_IOCTL_ERRNO(ret)));

    95

        return -EC_IOCTL_ERRNO(ret);

    96

    }

    97

    98

    return 0;

    99

}

   100

   101

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

   102

   103

void ecrt_slave_config_watchdog(ec_slave_config_t *sc,

   104

        uint16_t divider, uint16_t intervals)

   105

{

   106

    ec_ioctl_config_t data;

   107

    int ret;

   108

   109

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

   110

    data.config_index = sc->index;

   111

    data.watchdog_divider = divider;

   112

    data.watchdog_intervals = intervals;

   113

   114

    ret = ioctl(sc->master->fd, EC_IOCTL_SC_WATCHDOG, &data);

   115

    if (EC_IOCTL_IS_ERROR(ret)) {

   116

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

   117

                strerror(EC_IOCTL_ERRNO(ret)));

   118

    }

   119

}

   120

   121

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

   122

   123

void ecrt_slave_config_overlapping_pdos(ec_slave_config_t *sc,

   124

        uint8_t allow_overlapping_pdos)

   125

{

   126

    ec_ioctl_config_t data;

   127

   128

    memset(&data, 0, sizeof(ec_ioctl_config_t));

   129

    data.config_index = sc->index;

   130

    data.allow_overlapping_pdos = allow_overlapping_pdos;

   131

   132

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

   133

        fprintf(stderr, "Failed to config overlapping PDOs: %s\n",

   134

                strerror(errno));

   135

    }

   136

}

   137

   138

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

   139

   140

int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc,

   141

        uint8_t sync_index, uint16_t pdo_index)

   142

{

   143

    ec_ioctl_config_pdo_t data;

   144

    int ret;

   145

   146

    data.config_index = sc->index;

   147

    data.sync_index = sync_index;

   148

    data.index = pdo_index;

   149

   150

    ret = ioctl(sc->master->fd, EC_IOCTL_SC_ADD_PDO, &data);

   151

    if (EC_IOCTL_IS_ERROR(ret)) {

   152

        fprintf(stderr, "Failed to add PDO: %s\n",

   153

                strerror(EC_IOCTL_ERRNO(ret)));

   154

        return -EC_IOCTL_ERRNO(ret);

   155

    }

   156

   157

    return 0;

   158

}

   159

   160

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

   161

   162

void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc,

   163

        uint8_t sync_index)

   164

{

   165

    ec_ioctl_config_pdo_t data;

   166

    int ret;

   167

   168

    data.config_index = sc->index;

   169

    data.sync_index = sync_index;

   170

   171

    ret = ioctl(sc->master->fd, EC_IOCTL_SC_CLEAR_PDOS, &data);

   172

    if (EC_IOCTL_IS_ERROR(ret)) {

   173

        fprintf(stderr, "Failed to clear PDOs: %s\n",

   174

                strerror(EC_IOCTL_ERRNO(ret)));

   175

    }

   176

}

   177

   178

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

   179

   180

int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc,

   181

        uint16_t pdo_index, uint16_t entry_index, uint8_t entry_subindex,

   182

        uint8_t entry_bit_length)

   183

{

   184

    ec_ioctl_add_pdo_entry_t data;

   185

    int ret;

   186

   187

    data.config_index = sc->index;

   188

    data.pdo_index = pdo_index;

   189

    data.entry_index = entry_index;

   190

    data.entry_subindex = entry_subindex;

   191

    data.entry_bit_length = entry_bit_length;

   192

   193

    ret = ioctl(sc->master->fd, EC_IOCTL_SC_ADD_ENTRY, &data);

   194

    if (EC_IOCTL_IS_ERROR(ret)) {

   195

        fprintf(stderr, "Failed to add PDO entry: %s\n",

   196

                strerror(EC_IOCTL_ERRNO(ret)));

   197

        return -EC_IOCTL_ERRNO(ret);

   198

    }

   199

   200

    return 0;

   201

}

   202

   203

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

   204

   205

void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc,

   206

        uint16_t pdo_index)

   207

{

   208

    ec_ioctl_config_pdo_t data;

   209

    int ret;

   210

   211

    data.config_index = sc->index;

   212

    data.index = pdo_index;

   213

   214

    ret = ioctl(sc->master->fd, EC_IOCTL_SC_CLEAR_ENTRIES, &data);

   215

    if (EC_IOCTL_IS_ERROR(ret)) {

   216

        fprintf(stderr, "Failed to clear PDO entries: %s\n",

   217

                strerror(EC_IOCTL_ERRNO(ret)));

   218

    }

   219

}

   220

   221

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

   222

   223

int ecrt_slave_config_pdos(ec_slave_config_t *sc,

   224

        unsigned int n_syncs, const ec_sync_info_t syncs[])

   225

{

   226

    int ret;

   227

    unsigned int i, j, k;

   228

    const ec_sync_info_t *sync_info;

   229

    const ec_pdo_info_t *pdo_info;

   230

    const ec_pdo_entry_info_t *entry_info;

   231

   232

    if (!syncs)

   233

        return 0;

   234

   235

    for (i = 0; i < n_syncs; i++) {

   236

        sync_info = &syncs[i];

   237

   238

        if (sync_info->index == (uint8_t) EC_END)

   239

            break;

   240

   241

        if (sync_info->index >= EC_MAX_SYNC_MANAGERS) {

   242

            fprintf(stderr, "Invalid sync manager index %u!\n",

   243

                    sync_info->index);

   244

            return -ENOENT;

   245

        }

   246

   247

        ret = ecrt_slave_config_sync_manager(sc, sync_info->index,

   248

                sync_info->dir, sync_info->watchdog_mode);

   249

        if (ret)

   250

            return ret;

   251

   252

        ecrt_slave_config_pdo_assign_clear(sc, sync_info->index);

   253

   254

        if (sync_info->n_pdos && sync_info->pdos) {

   255

   256

            for (j = 0; j < sync_info->n_pdos; j++) {

   257

                pdo_info = &sync_info->pdos[j];

   258

   259

                ret = ecrt_slave_config_pdo_assign_add(

   260

                        sc, sync_info->index, pdo_info->index);

   261

                if (ret)

   262

                    return ret;

   263

   264

                if (pdo_info->n_entries && pdo_info->entries) {

   265

                    ecrt_slave_config_pdo_mapping_clear(sc, pdo_info->index);

   266

   267

                    for (k = 0; k < pdo_info->n_entries; k++) {

   268

                        entry_info = &pdo_info->entries[k];

   269

   270

                        ret = ecrt_slave_config_pdo_mapping_add(sc,

   271

                                pdo_info->index, entry_info->index,

   272

                                entry_info->subindex,

   273

                                entry_info->bit_length);

   274

                        if (ret)

   275

                            return ret;

   276

                    }

   277

                }

   278

            }

   279

        }

   280

    }

   281

   282

    return 0;

   283

}

   284

   285

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

   286

   287

int ecrt_slave_config_reg_pdo_entry(

   288

        ec_slave_config_t *sc,

   289

        uint16_t index,

   290

        uint8_t subindex,

   291

        ec_domain_t *domain,

   292

        unsigned int *bit_position

   293

        )

   294

{

   295

    ec_ioctl_reg_pdo_entry_t data;

   296

    int ret;

   297

   298

    data.config_index = sc->index;

   299

    data.entry_index = index;

   300

    data.entry_subindex = subindex;

   301

    data.domain_index = domain->index;

   302

   303

    ret = ioctl(sc->master->fd, EC_IOCTL_SC_REG_PDO_ENTRY, &data);

   304

    if (EC_IOCTL_IS_ERROR(ret)) {

   305

        fprintf(stderr, "Failed to register PDO entry: %s\n",

   306

                strerror(EC_IOCTL_ERRNO(ret)));

   307

        return -EC_IOCTL_ERRNO(ret);

   308

    }

   309

   310

    if (bit_position) {

   311

        *bit_position = data.bit_position;

   312

    } else {

   313

        if (data.bit_position) {

   314

            fprintf(stderr, "PDO entry 0x%04X:%02X does not byte-align "

   315

                    "in config %u:%u.\n", index, subindex,

   316

                    sc->alias, sc->position);

   317

            return -EFAULT;

   318

        }

   319

    }

   320

   321

    return ret;

   322

}

   323

   324

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

   325

   326

int ecrt_slave_config_reg_pdo_entry_pos(

   327

        ec_slave_config_t *sc,

   328

        uint8_t sync_index,

   329

        unsigned int pdo_pos,

   330

        unsigned int entry_pos,

   331

        ec_domain_t *domain,

   332

        unsigned int *bit_position

   333

        )

   334

{

   335

    ec_ioctl_reg_pdo_pos_t io;

   336

    int ret;

   337

   338

    io.config_index = sc->index;

   339

    io.sync_index = sync_index;

   340

    io.pdo_pos = pdo_pos;

   341

    io.entry_pos = entry_pos;

   342

    io.domain_index = domain->index;

   343

   344

    ret = ioctl(sc->master->fd, EC_IOCTL_SC_REG_PDO_POS, &io);

   345

    if (EC_IOCTL_IS_ERROR(ret)) {

   346

        fprintf(stderr, "Failed to register PDO entry: %s\n",

   347

                strerror(EC_IOCTL_ERRNO(ret)));

   348

        return -EC_IOCTL_ERRNO(ret);

   349

    }

   350

   351

    if (bit_position) {

   352

        *bit_position = io.bit_position;

   353

    } else {

   354

        if (io.bit_position) {

   355

            fprintf(stderr, "PDO entry %u/%u/%u does not byte-align "

   356

                    "in config %u:%u.\n", sync_index, pdo_pos, entry_pos,

   357

                    sc->alias, sc->position);

   358

            return -EFAULT;

   359

        }

   360

    }

   361

   362

    return ret;

   363

}

   364

   365

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

   366

   367

void ecrt_slave_config_dc(ec_slave_config_t *sc, uint16_t assign_activate,

   368

        uint32_t sync0_cycle_time, int32_t sync0_shift_time,

   369

        uint32_t sync1_cycle_time, int32_t sync1_shift_time)

   370

{

   371

    ec_ioctl_config_t data;

   372

    int ret;

   373

   374

    data.config_index = sc->index;

   375

    data.dc_assign_activate = assign_activate;

   376

    data.dc_sync[0].cycle_time = sync0_cycle_time;

   377

    data.dc_sync[0].shift_time = sync0_shift_time;

   378

    data.dc_sync[1].cycle_time = sync1_cycle_time;

   379

    data.dc_sync[1].shift_time = sync1_shift_time;

   380

   381

    ret = ioctl(sc->master->fd, EC_IOCTL_SC_DC, &data);

   382

    if (EC_IOCTL_IS_ERROR(ret)) {

   383

        fprintf(stderr, "Failed to set DC parameters: %s\n",

   384

                strerror(EC_IOCTL_ERRNO(ret)));

   385

    }

   386

}

   387

   388

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

   389

   390

int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index,

   391

        uint8_t subindex, const uint8_t *sdo_data, size_t size)

   392

{

   393

    ec_ioctl_sc_sdo_t data;

   394

    int ret;

   395

   396

    data.config_index = sc->index;

   397

    data.index = index;

   398

    data.subindex = subindex;

   399

    data.data = sdo_data;

   400

    data.size = size;

   401

    data.complete_access = 0;

   402

   403

    ret = ioctl(sc->master->fd, EC_IOCTL_SC_SDO, &data);

   404

    if (EC_IOCTL_IS_ERROR(ret)) {

   405

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

   406

                strerror(EC_IOCTL_ERRNO(ret)));

   407

        return -EC_IOCTL_ERRNO(ret);

   408

    }

   409

   410

    return 0;

   411

}

   412

   413

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

   414

   415

int ecrt_slave_config_complete_sdo(ec_slave_config_t *sc, uint16_t index,

   416

        const uint8_t *sdo_data, size_t size)

   417

{

   418

    ec_ioctl_sc_sdo_t data;

   419

    int ret;

   420

   421

    data.config_index = sc->index;

   422

    data.index = index;

   423

    data.subindex = 0;

   424

    data.data = sdo_data;

   425

    data.size = size;

   426

    data.complete_access = 1;

   427

   428

    ret = ioctl(sc->master->fd, EC_IOCTL_SC_SDO, &data);

   429

    if (EC_IOCTL_IS_ERROR(ret)) {

   430

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

   431

                strerror(EC_IOCTL_ERRNO(ret)));

   432

        return -EC_IOCTL_ERRNO(ret);

   433

    }

   434

   435

    return 0;

   436

}

   437

   438

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

   439

   440

int ecrt_slave_config_sdo8(ec_slave_config_t *sc, uint16_t index,

   441

        uint8_t subindex, uint8_t value)

   442

{

   443

    uint8_t data[1];

   444

   445

    EC_WRITE_U8(data, value);

   446

    return ecrt_slave_config_sdo(sc, index, subindex, data, 1);

   447

}

   448

   449

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

   450

   451

int ecrt_slave_config_sdo16(ec_slave_config_t *sc, uint16_t index,

   452

        uint8_t subindex, uint16_t value)

   453

{

   454

    uint8_t data[2];

   455

   456

    EC_WRITE_U16(data, value);

   457

    return ecrt_slave_config_sdo(sc, index, subindex, data, 2);

   458

}

   459

   460

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

   461

   462

int ecrt_slave_config_sdo32(ec_slave_config_t *sc, uint16_t index,

   463

        uint8_t subindex, uint32_t value)

   464

{

   465

    uint8_t data[4];

   466

   467

    EC_WRITE_U32(data, value);

   468

    return ecrt_slave_config_sdo(sc, index, subindex, data, 4);

   469

}

   470

   471

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

   472

   473

int ecrt_slave_config_emerg_size(ec_slave_config_t *sc, size_t elements)

   474

{

   475

    ec_ioctl_sc_emerg_t io;

   476

    int ret;

   477

   478

    io.config_index = sc->index;

   479

    io.size = elements;

   480

   481

    ret = ioctl(sc->master->fd, EC_IOCTL_SC_EMERG_SIZE, &io);

   482

    if (EC_IOCTL_IS_ERROR(ret)) {

   483

        fprintf(stderr, "Failed to set emergency ring size: %s\n",

   484

                strerror(EC_IOCTL_ERRNO(ret)));

   485

        return -EC_IOCTL_ERRNO(ret);

   486

    }

   487

   488

    return 0;

   489

}

   490

   491

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

   492

   493

int ecrt_slave_config_emerg_pop(ec_slave_config_t *sc, uint8_t *target)

   494

{

   495

    ec_ioctl_sc_emerg_t io;

   496

    int ret;

   497

   498

    io.config_index = sc->index;

   499

    io.target = target;

   500

   501

    ret = ioctl(sc->master->fd, EC_IOCTL_SC_EMERG_POP, &io);

   502

    if (EC_IOCTL_IS_ERROR(ret)) {

   503

        if (EC_IOCTL_ERRNO(ret) != ENOENT) {