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

int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc,

   124

        uint8_t sync_index, uint16_t pdo_index)

   125

{

   126

    ec_ioctl_config_pdo_t data;

   127

    int ret;

   128

   129

    data.config_index = sc->index;

   130

    data.sync_index = sync_index;

   131

    data.index = pdo_index;

   132

   133

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

   134

    if (EC_IOCTL_IS_ERROR(ret)) {

   135

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

   136

                strerror(EC_IOCTL_ERRNO(ret)));

   137

        return -EC_IOCTL_ERRNO(ret);

   138

    }

   139

   140

    return 0;

   141

}

   142

   143

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

   144

   145

void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc,

   146

        uint8_t sync_index)

   147

{

   148

    ec_ioctl_config_pdo_t data;

   149

    int ret;

   150

   151

    data.config_index = sc->index;

   152

    data.sync_index = sync_index;

   153

   154

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

   155

    if (EC_IOCTL_IS_ERROR(ret)) {

   156

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

   157

                strerror(EC_IOCTL_ERRNO(ret)));

   158

    }

   159

}

   160

   161

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

   162

   163

int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc,

   164

        uint16_t pdo_index, uint16_t entry_index, uint8_t entry_subindex,

   165

        uint8_t entry_bit_length)

   166

{

   167

    ec_ioctl_add_pdo_entry_t data;

   168

    int ret;

   169

   170

    data.config_index = sc->index;

   171

    data.pdo_index = pdo_index;

   172

    data.entry_index = entry_index;

   173

    data.entry_subindex = entry_subindex;

   174

    data.entry_bit_length = entry_bit_length;

   175

   176

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

   177

    if (EC_IOCTL_IS_ERROR(ret)) {

   178

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

   179

                strerror(EC_IOCTL_ERRNO(ret)));

   180

        return -EC_IOCTL_ERRNO(ret);

   181

    }

   182

   183

    return 0;

   184

}

   185

   186

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

   187

   188

void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc,

   189

        uint16_t pdo_index)

   190

{

   191

    ec_ioctl_config_pdo_t data;

   192

    int ret;

   193

   194

    data.config_index = sc->index;

   195

    data.index = pdo_index;

   196

   197

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

   198

    if (EC_IOCTL_IS_ERROR(ret)) {

   199

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

   200

                strerror(EC_IOCTL_ERRNO(ret)));

   201

    }

   202

}

   203

   204

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

   205

   206

int ecrt_slave_config_pdos(ec_slave_config_t *sc,

   207

        unsigned int n_syncs, const ec_sync_info_t syncs[])

   208

{

   209

    int ret;

   210

    unsigned int i, j, k;

   211

    const ec_sync_info_t *sync_info;

   212

    const ec_pdo_info_t *pdo_info;

   213

    const ec_pdo_entry_info_t *entry_info;

   214

   215

    if (!syncs)

   216

        return 0;

   217

   218

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

   219

        sync_info = &syncs[i];

   220

   221

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

   222

            break;

   223

   224

        if (sync_info->index >= EC_MAX_SYNC_MANAGERS) {

   225

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

   226

                    sync_info->index);

   227

            return -ENOENT;

   228

        }

   229

   230

        ret = ecrt_slave_config_sync_manager(sc, sync_info->index,

   231

                sync_info->dir, sync_info->watchdog_mode);

   232

        if (ret)

   233

            return ret;

   234

   235

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

   236

            ecrt_slave_config_pdo_assign_clear(sc, sync_info->index);

   237

   238

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

   239

                pdo_info = &sync_info->pdos[j];

   240

   241

                ret = ecrt_slave_config_pdo_assign_add(

   242

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

   243

                if (ret)

   244

                    return ret;

   245

   246

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

   247

                    ecrt_slave_config_pdo_mapping_clear(sc, pdo_info->index);

   248

   249

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

   250

                        entry_info = &pdo_info->entries[k];

   251

   252

                        ret = ecrt_slave_config_pdo_mapping_add(sc,

   253

                                pdo_info->index, entry_info->index,

   254

                                entry_info->subindex,

   255

                                entry_info->bit_length);

   256

                        if (ret)

   257

                            return ret;

   258

                    }

   259

                }

   260

            }

   261

        }

   262

    }

   263

   264

    return 0;

   265

}

   266

   267

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

   268

   269

int ecrt_slave_config_reg_pdo_entry(

   270

        ec_slave_config_t *sc,

   271

        uint16_t index,

   272

        uint8_t subindex,

   273

        ec_domain_t *domain,

   274

        unsigned int *bit_position

   275

        )

   276

{

   277

    ec_ioctl_reg_pdo_entry_t data;

   278

    int ret;

   279

   280

    data.config_index = sc->index;

   281

    data.entry_index = index;

   282

    data.entry_subindex = subindex;

   283

    data.domain_index = domain->index;

   284

   285

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

   286

    if (EC_IOCTL_IS_ERROR(ret)) {

   287

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

   288

                strerror(EC_IOCTL_ERRNO(ret)));

   289

        return -EC_IOCTL_ERRNO(ret);

   290

    }

   291

   292

    if (bit_position) {

   293

        *bit_position = data.bit_position;

   294

    } else {

   295

        if (data.bit_position) {

   296

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

   297

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

   298

                    sc->alias, sc->position);

   299

            return -EFAULT;

   300

        }

   301

    }

   302

   303

    return ret;

   304

}

   305

   306

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

   307

   308

int ecrt_slave_config_reg_pdo_entry_pos(

   309

        ec_slave_config_t *sc,

   310

        uint8_t sync_index,

   311

        unsigned int pdo_pos,

   312

        unsigned int entry_pos,

   313

        ec_domain_t *domain,

   314

        unsigned int *bit_position

   315

        )

   316

{

   317

    ec_ioctl_reg_pdo_pos_t io;

   318

    int ret;

   319

   320

    io.config_index = sc->index;

   321

    io.sync_index = sync_index;

   322

    io.pdo_pos = pdo_pos;

   323

    io.entry_pos = entry_pos;

   324

    io.domain_index = domain->index;

   325

   326

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

   327

    if (EC_IOCTL_IS_ERROR(ret)) {

   328

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

   329

                strerror(EC_IOCTL_ERRNO(ret)));

   330

        return -EC_IOCTL_ERRNO(ret);

   331

    }

   332

   333

    if (bit_position) {

   334

        *bit_position = io.bit_position;

   335

    } else {

   336

        if (io.bit_position) {

   337

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

   338

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

   339

                    sc->alias, sc->position);

   340

            return -EFAULT;

   341

        }

   342

    }

   343

   344

    return ret;

   345

}

   346

   347

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

   348

   349

void ecrt_slave_config_dc(ec_slave_config_t *sc, uint16_t assign_activate,

   350

        uint32_t sync0_cycle_time, int32_t sync0_shift_time,

   351

        uint32_t sync1_cycle_time, int32_t sync1_shift_time)

   352

{

   353

    ec_ioctl_config_t data;

   354

    int ret;

   355

   356

    data.config_index = sc->index;

   357

    data.dc_assign_activate = assign_activate;

   358

    data.dc_sync[0].cycle_time = sync0_cycle_time;

   359

    data.dc_sync[0].shift_time = sync0_shift_time;

   360

    data.dc_sync[1].cycle_time = sync1_cycle_time;

   361

    data.dc_sync[1].shift_time = sync1_shift_time;

   362

   363

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

   364

    if (EC_IOCTL_IS_ERROR(ret)) {

   365

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

   366

                strerror(EC_IOCTL_ERRNO(ret)));

   367

    }

   368

}

   369

   370

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

   371

   372

int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index,

   373

        uint8_t subindex, const uint8_t *sdo_data, size_t size)

   374

{

   375

    ec_ioctl_sc_sdo_t data;

   376

    int ret;

   377

   378

    data.config_index = sc->index;

   379

    data.index = index;

   380

    data.subindex = subindex;

   381

    data.data = sdo_data;

   382

    data.size = size;

   383

    data.complete_access = 0;

   384

   385

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

   386

    if (EC_IOCTL_IS_ERROR(ret)) {

   387

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

   388

                strerror(EC_IOCTL_ERRNO(ret)));

   389

        return -EC_IOCTL_ERRNO(ret);

   390

    }

   391

   392

    return 0;

   393

}

   394

   395

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

   396

   397

int ecrt_slave_config_complete_sdo(ec_slave_config_t *sc, uint16_t index,

   398

        const uint8_t *sdo_data, size_t size)

   399

{

   400

    ec_ioctl_sc_sdo_t data;

   401

    int ret;

   402

   403

    data.config_index = sc->index;

   404

    data.index = index;

   405

    data.subindex = 0;

   406

    data.data = sdo_data;

   407

    data.size = size;

   408

    data.complete_access = 1;

   409

   410

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

   411

    if (EC_IOCTL_IS_ERROR(ret)) {

   412

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

   413

                strerror(EC_IOCTL_ERRNO(ret)));

   414

        return -EC_IOCTL_ERRNO(ret);

   415

    }

   416

   417

    return 0;

   418

}

   419

   420

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

   421

   422

int ecrt_slave_config_sdo8(ec_slave_config_t *sc, uint16_t index,

   423

        uint8_t subindex, uint8_t value)

   424

{

   425

    uint8_t data[1];

   426

   427

    EC_WRITE_U8(data, value);

   428

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

   429

}

   430

   431

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

   432

   433

int ecrt_slave_config_sdo16(ec_slave_config_t *sc, uint16_t index,

   434

        uint8_t subindex, uint16_t value)

   435

{

   436

    uint8_t data[2];

   437

   438

    EC_WRITE_U16(data, value);

   439

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

   440

}

   441

   442

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

   443

   444

int ecrt_slave_config_sdo32(ec_slave_config_t *sc, uint16_t index,

   445

        uint8_t subindex, uint32_t value)

   446

{

   447

    uint8_t data[4];

   448

   449

    EC_WRITE_U32(data, value);

   450

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

   451

}

   452

   453

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

   454

   455

int ecrt_slave_config_emerg_size(ec_slave_config_t *sc, size_t elements)

   456

{

   457

    ec_ioctl_sc_emerg_t io;

   458

    int ret;

   459

   460

    io.config_index = sc->index;

   461

    io.size = elements;

   462

   463

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

   464

    if (EC_IOCTL_IS_ERROR(ret)) {

   465

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

   466

                strerror(EC_IOCTL_ERRNO(ret)));

   467

        return -EC_IOCTL_ERRNO(ret);

   468

    }

   469

   470

    return 0;

   471

}

   472

   473

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

   474

   475

int ecrt_slave_config_emerg_pop(ec_slave_config_t *sc, uint8_t *target)

   476

{

   477

    ec_ioctl_sc_emerg_t io;

   478

    int ret;

   479

   480

    io.config_index = sc->index;

   481

    io.target = target;

   482

   483

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

   484

    if (EC_IOCTL_IS_ERROR(ret)) {

   485

        if (EC_IOCTL_ERRNO(ret) != ENOENT) {

   486

            fprintf(stderr, "Failed to get emergency message: %s\n",

   487

                    strerror(EC_IOCTL_ERRNO(ret)));

   488

        }

   489

        return -EC_IOCTL_ERRNO(ret);

   490

    }

   491

   492

    return 0;

   493

}

   494

   495

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

   496

   497

int ecrt_slave_config_emerg_clear(ec_slave_config_t *sc)

   498

{

   499

    ec_ioctl_sc_emerg_t io;

   500

    int ret;

   501

   502

    io.config_index = sc->index;