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

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

   253

            ecrt_slave_config_pdo_assign_clear(sc, sync_info->index);

   254

   255

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

   256

                pdo_info = &sync_info->pdos[j];

   257

   258

                ret = ecrt_slave_config_pdo_assign_add(

   259

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

   260

                if (ret)

   261

                    return ret;

   262

   263

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

   264

                    ecrt_slave_config_pdo_mapping_clear(sc, pdo_info->index);

   265

   266

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

   267

                        entry_info = &pdo_info->entries[k];

   268

   269

                        ret = ecrt_slave_config_pdo_mapping_add(sc,

   270

                                pdo_info->index, entry_info->index,

   271

                                entry_info->subindex,

   272

                                entry_info->bit_length);

   273

                        if (ret)

   274

                            return ret;

   275

                    }

   276

                }

   277

            }

   278

        }

   279

    }

   280

   281

    return 0;

   282

}

   283

   284

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

   285

   286

int ecrt_slave_config_reg_pdo_entry(

   287

        ec_slave_config_t *sc,

   288

        uint16_t index,

   289

        uint8_t subindex,

   290

        ec_domain_t *domain,

   291

        unsigned int *bit_position

   292

        )

   293

{

   294

    ec_ioctl_reg_pdo_entry_t data;

   295

    int ret;

   296

   297

    data.config_index = sc->index;

   298

    data.entry_index = index;

   299

    data.entry_subindex = subindex;

   300

    data.domain_index = domain->index;

   301

   302

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

   303

    if (EC_IOCTL_IS_ERROR(ret)) {

   304

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

   305

                strerror(EC_IOCTL_ERRNO(ret)));

   306

        return -EC_IOCTL_ERRNO(ret);

   307

    }

   308

   309

    if (bit_position) {

   310

        *bit_position = data.bit_position;

   311

    } else {

   312

        if (data.bit_position) {

   313

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

   314

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

   315

                    sc->alias, sc->position);

   316

            return -EFAULT;

   317

        }

   318

    }

   319

   320

    return ret;

   321

}

   322

   323

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

   324

   325

int ecrt_slave_config_reg_pdo_entry_pos(

   326

        ec_slave_config_t *sc,

   327

        uint8_t sync_index,

   328

        unsigned int pdo_pos,

   329

        unsigned int entry_pos,

   330

        ec_domain_t *domain,

   331

        unsigned int *bit_position

   332

        )

   333

{

   334

    ec_ioctl_reg_pdo_pos_t io;

   335

    int ret;

   336

   337

    io.config_index = sc->index;

   338

    io.sync_index = sync_index;

   339

    io.pdo_pos = pdo_pos;

   340

    io.entry_pos = entry_pos;

   341

    io.domain_index = domain->index;

   342

   343

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

   344

    if (EC_IOCTL_IS_ERROR(ret)) {

   345

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

   346

                strerror(EC_IOCTL_ERRNO(ret)));

   347

        return -EC_IOCTL_ERRNO(ret);

   348

    }

   349

   350

    if (bit_position) {

   351

        *bit_position = io.bit_position;

   352

    } else {

   353

        if (io.bit_position) {

   354

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

   355

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

   356

                    sc->alias, sc->position);

   357

            return -EFAULT;

   358

        }

   359

    }

   360

   361

    return ret;

   362

}

   363

   364

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

   365

   366

void ecrt_slave_config_dc(ec_slave_config_t *sc, uint16_t assign_activate,

   367

        uint32_t sync0_cycle_time, int32_t sync0_shift_time,

   368

        uint32_t sync1_cycle_time, int32_t sync1_shift_time)

   369

{

   370

    ec_ioctl_config_t data;

   371

    int ret;

   372

   373

    data.config_index = sc->index;

   374

    data.dc_assign_activate = assign_activate;

   375

    data.dc_sync[0].cycle_time = sync0_cycle_time;

   376

    data.dc_sync[0].shift_time = sync0_shift_time;

   377

    data.dc_sync[1].cycle_time = sync1_cycle_time;

   378

    data.dc_sync[1].shift_time = sync1_shift_time;

   379

   380

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

   381

    if (EC_IOCTL_IS_ERROR(ret)) {

   382

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

   383

                strerror(EC_IOCTL_ERRNO(ret)));

   384

    }

   385

}

   386

   387

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

   388

   389

int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index,

   390

        uint8_t subindex, const uint8_t *sdo_data, size_t size)

   391

{

   392

    ec_ioctl_sc_sdo_t data;

   393

    int ret;

   394

   395

    data.config_index = sc->index;

   396

    data.index = index;

   397

    data.subindex = subindex;

   398

    data.data = sdo_data;

   399

    data.size = size;

   400

    data.complete_access = 0;

   401

   402

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

   403

    if (EC_IOCTL_IS_ERROR(ret)) {

   404

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

   405

                strerror(EC_IOCTL_ERRNO(ret)));

   406

        return -EC_IOCTL_ERRNO(ret);

   407

    }

   408

   409

    return 0;

   410

}

   411

   412

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

   413

   414

int ecrt_slave_config_complete_sdo(ec_slave_config_t *sc, uint16_t index,

   415

        const uint8_t *sdo_data, size_t size)

   416

{

   417

    ec_ioctl_sc_sdo_t data;

   418

    int ret;

   419

   420

    data.config_index = sc->index;

   421

    data.index = index;

   422

    data.subindex = 0;

   423

    data.data = sdo_data;

   424

    data.size = size;

   425

    data.complete_access = 1;

   426

   427

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

   428

    if (EC_IOCTL_IS_ERROR(ret)) {

   429

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

   430

                strerror(EC_IOCTL_ERRNO(ret)));

   431

        return -EC_IOCTL_ERRNO(ret);

   432

    }

   433

   434

    return 0;

   435

}

   436

   437

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

   438

   439

int ecrt_slave_config_sdo8(ec_slave_config_t *sc, uint16_t index,

   440

        uint8_t subindex, uint8_t value)

   441

{

   442

    uint8_t data[1];

   443

   444

    EC_WRITE_U8(data, value);

   445

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

   446

}

   447

   448

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

   449

   450

int ecrt_slave_config_sdo16(ec_slave_config_t *sc, uint16_t index,

   451

        uint8_t subindex, uint16_t value)

   452

{

   453

    uint8_t data[2];

   454

   455

    EC_WRITE_U16(data, value);

   456

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

   457

}

   458

   459

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

   460

   461

int ecrt_slave_config_sdo32(ec_slave_config_t *sc, uint16_t index,

   462

        uint8_t subindex, uint32_t value)

   463

{

   464

    uint8_t data[4];

   465

   466

    EC_WRITE_U32(data, value);

   467

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

   468

}

   469

   470

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

   471

   472

int ecrt_slave_config_emerg_size(ec_slave_config_t *sc, size_t elements)

   473

{

   474

    ec_ioctl_sc_emerg_t io;

   475

    int ret;

   476

   477

    io.config_index = sc->index;

   478

    io.size = elements;

   479

   480

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

   481

    if (EC_IOCTL_IS_ERROR(ret)) {

   482

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

   483

                strerror(EC_IOCTL_ERRNO(ret)));

   484

        return -EC_IOCTL_ERRNO(ret);

   485

    }

   486

   487

    return 0;

   488

}

   489

   490

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

   491

   492

int ecrt_slave_config_emerg_pop(ec_slave_config_t *sc, uint8_t *target)

   493

{

   494

    ec_ioctl_sc_emerg_t io;

   495

    int ret;

   496

   497

    io.config_index = sc->index;

   498

    io.target = target;

   499

   500

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

   501

    if (EC_IOCTL_IS_ERROR(ret)) {

   502

        if (EC_IOCTL_ERRNO(ret) != ENOENT) {

   503