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

    37

#include "slave_config.h"

    38

#include "domain.h"

    39

#include "sdo_request.h"

    40

#include "voe_handler.h"

    41

#include "master.h"

    42

#include "master/ioctl.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_voe_handler_t *v, *next_v;

    50

    51

    r = sc->first_sdo_request;

    52

    while (r) {

    53

        next_r = r->next;

    54

        ec_sdo_request_clear(r);

    55

        r = next_r;

    56

    }

    57

    58

    59

    v = sc->first_voe_handler;

    60

    while (v) {

    61

        next_v = v->next;

    62

        ec_voe_handler_clear(v);

    63

        v = next_v;

    64

    }

    65

}

    66

    67

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

    68

    69

int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index,

    70

        ec_direction_t dir, ec_watchdog_mode_t watchdog_mode)

    71

{

    72

    ec_ioctl_config_t data;

    73

    74

    if (sync_index >= EC_MAX_SYNC_MANAGERS)

    75

        return -ENOENT;

    76

    77

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

    78

    data.config_index = sc->index;

    79

    data.syncs[sync_index].dir = dir;

    80

    data.syncs[sync_index].watchdog_mode = watchdog_mode;

    81

    data.syncs[sync_index].config_this = 1;

    82

    83

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

    84

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

    85

                strerror(errno));

    86

        return -1; // FIXME

    87

    }

    88

    89

    return 0;

    90

}

    91

    92

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

    93

    94

void ecrt_slave_config_watchdog(ec_slave_config_t *sc,

    95

        uint16_t divider, uint16_t intervals)

    96

{

    97

    ec_ioctl_config_t data;

    98

    99

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

   100

    data.config_index = sc->index;

   101

    data.watchdog_divider = divider;

   102

    data.watchdog_intervals = intervals;

   103

   104

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

   105

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

   106

                strerror(errno));

   107

    }

   108

}

   109

   110

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

   111

   112

int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc,

   113

        uint8_t sync_index, uint16_t pdo_index)

   114

{

   115

    ec_ioctl_config_pdo_t data;

   116

   117

    data.config_index = sc->index;

   118

    data.sync_index = sync_index;

   119

    data.index = pdo_index;

   120

   121

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

   122

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

   123

                strerror(errno));

   124

        return -1;  // FIXME

   125

    }

   126

   127

    return 0;

   128

}

   129

   130

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

   131

   132

void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc,

   133

        uint8_t sync_index)

   134

{

   135

    ec_ioctl_config_pdo_t data;

   136

   137

    data.config_index = sc->index;

   138

    data.sync_index = sync_index;

   139

   140

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

   141

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

   142

                strerror(errno));

   143

    }

   144

}

   145

   146

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

   147

   148

int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc,

   149

        uint16_t pdo_index, uint16_t entry_index, uint8_t entry_subindex,

   150

        uint8_t entry_bit_length)

   151

{

   152

    ec_ioctl_add_pdo_entry_t data;

   153

   154

    data.config_index = sc->index;

   155

    data.pdo_index = pdo_index;

   156

    data.entry_index = entry_index;

   157

    data.entry_subindex = entry_subindex;

   158

    data.entry_bit_length = entry_bit_length;

   159

   160

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

   161

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

   162

                strerror(errno));

   163

        return -1;  // FIXME

   164

    }

   165

   166

    return 0;

   167

}

   168

   169

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

   170

   171

void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc,

   172

        uint16_t pdo_index)

   173

{

   174

    ec_ioctl_config_pdo_t data;

   175

   176

    data.config_index = sc->index;

   177

    data.index = pdo_index;

   178

   179

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

   180

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

   181

                strerror(errno));

   182

    }

   183

}

   184

   185

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

   186

   187

int ecrt_slave_config_pdos(ec_slave_config_t *sc,

   188

        unsigned int n_syncs, const ec_sync_info_t syncs[])

   189

{

   190

    int ret;

   191

    unsigned int i, j, k;

   192

    const ec_sync_info_t *sync_info;

   193

    const ec_pdo_info_t *pdo_info;

   194

    const ec_pdo_entry_info_t *entry_info;

   195

   196

    if (!syncs)

   197

        return 0;

   198

   199

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

   200

        sync_info = &syncs[i];

   201

   202

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

   203

            break;

   204

   205

        if (sync_info->index >= EC_MAX_SYNC_MANAGERS) {

   206

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

   207

                    sync_info->index);

   208

            return -ENOENT;

   209

        }

   210

   211

        ret = ecrt_slave_config_sync_manager(sc, sync_info->index,

   212

                sync_info->dir, sync_info->watchdog_mode);

   213

        if (ret)

   214

            return ret;

   215

   216

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

   217

            ecrt_slave_config_pdo_assign_clear(sc, sync_info->index);

   218

   219

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

   220

                pdo_info = &sync_info->pdos[j];

   221

   222

                ret = ecrt_slave_config_pdo_assign_add(

   223

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

   224

                if (ret)

   225

                    return ret;

   226

   227

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

   228

                    ecrt_slave_config_pdo_mapping_clear(sc, pdo_info->index);

   229

   230

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

   231

                        entry_info = &pdo_info->entries[k];

   232

   233

                        ret = ecrt_slave_config_pdo_mapping_add(sc,

   234

                                pdo_info->index, entry_info->index,

   235

                                entry_info->subindex,

   236

                                entry_info->bit_length);

   237

                        if (ret)

   238

                            return ret;

   239

                    }

   240

                }

   241

            }

   242

        }

   243

    }

   244

   245

    return 0;

   246

}

   247

   248

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

   249

   250

int ecrt_slave_config_reg_pdo_entry(

   251

        ec_slave_config_t *sc,

   252

        uint16_t index,

   253

        uint8_t subindex,

   254

        ec_domain_t *domain,

   255

        unsigned int *bit_position

   256

        )

   257

{

   258

    ec_ioctl_reg_pdo_entry_t data;

   259

    int ret;

   260

   261

    data.config_index = sc->index;

   262

    data.entry_index = index;

   263

    data.entry_subindex = subindex;

   264

    data.domain_index = domain->index;

   265

   266

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

   267

    if (ret == -1) {

   268

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

   269

                strerror(errno));

   270

        return -2; // FIXME

   271

    }

   272

   273

    if (bit_position) {

   274

        *bit_position = data.bit_position;

   275

    } else {

   276

        if (data.bit_position) {

   277

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

   278

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

   279

                    sc->alias, sc->position);

   280

            return -3; // FIXME

   281

        }

   282

    }

   283

   284

    return ret;

   285

}

   286

   287

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

   288

   289

void ecrt_slave_config_dc(ec_slave_config_t *sc, uint16_t assign_activate,

   290

        uint32_t sync0_cycle_time, uint32_t sync0_shift_time,

   291

        uint32_t sync1_cycle_time, uint32_t sync1_shift_time)

   292

{

   293

    ec_ioctl_config_t data;

   294

   295

    data.config_index = sc->index;

   296

    data.dc_assign_activate = assign_activate;

   297

    data.dc_sync[0].cycle_time = sync0_cycle_time;

   298

    data.dc_sync[0].shift_time = sync0_shift_time;

   299

    data.dc_sync[1].cycle_time = sync1_cycle_time;

   300

    data.dc_sync[1].shift_time = sync1_shift_time;

   301

   302

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

   303

        fprintf(stderr, "Failed to set assign_activate word.\n");

   304

    }

   305

}

   306

   307

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

   308

   309

int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index,

   310

        uint8_t subindex, const uint8_t *sdo_data, size_t size)

   311

{

   312

    ec_ioctl_sc_sdo_t data;

   313

   314

    data.config_index = sc->index;

   315

    data.index = index;

   316

    data.subindex = subindex;

   317

    data.data = sdo_data;

   318

    data.size = size;

   319

    data.complete_access = 0;

   320

   321

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

   322

        fprintf(stderr, "Failed to configure SDO.\n");

   323

        return -1; // FIXME

   324

    }

   325

   326

    return 0;

   327

}

   328

   329

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

   330

   331

int ecrt_slave_config_complete_sdo(ec_slave_config_t *sc, uint16_t index,

   332

        const uint8_t *sdo_data, size_t size)

   333

{

   334

    ec_ioctl_sc_sdo_t data;

   335

   336

    data.config_index = sc->index;

   337

    data.index = index;

   338

    data.subindex = 0;

   339

    data.data = sdo_data;

   340

    data.size = size;

   341

    data.complete_access = 1;

   342

   343

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

   344

        fprintf(stderr, "Failed to configure SDO.\n");

   345

        return -1; // FIXME

   346

    }

   347

   348

    return 0;

   349

}

   350

   351

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

   352

   353

int ecrt_slave_config_sdo8(ec_slave_config_t *sc, uint16_t index,

   354

        uint8_t subindex, uint8_t value)

   355

{

   356

    uint8_t data[1];

   357

   358

    EC_WRITE_U8(data, value);

   359

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

   360

}

   361

   362

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

   363

   364

int ecrt_slave_config_sdo16(ec_slave_config_t *sc, uint16_t index,

   365

        uint8_t subindex, uint16_t value)

   366

{

   367

    uint8_t data[2];

   368

   369

    EC_WRITE_U16(data, value);

   370

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

   371

}

   372

   373

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

   374

   375

int ecrt_slave_config_sdo32(ec_slave_config_t *sc, uint16_t index,

   376

        uint8_t subindex, uint32_t value)

   377

{

   378

    uint8_t data[4];

   379

   380

    EC_WRITE_U32(data, value);

   381

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

   382

}

   383

   384

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

   385

   386

void ec_slave_config_add_sdo_request(ec_slave_config_t *sc,

   387

        ec_sdo_request_t *req)

   388

{

   389

    if (sc->first_sdo_request) {

   390

        ec_sdo_request_t *r = sc->first_sdo_request;

   391

        while (r->next) {

   392

            r = r->next;

   393

        }

   394

        r->next = req;

   395

    } else {

   396

        sc->first_sdo_request = req;

   397

    }

   398

}

   399

   400

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

   401

   402

ec_sdo_request_t *ecrt_slave_config_create_sdo_request(ec_slave_config_t *sc,

   403

        uint16_t index, uint8_t subindex, size_t size)

   404

{

   405

    ec_ioctl_sdo_request_t data;

   406

    ec_sdo_request_t *req;

   407

   408

    req = malloc(sizeof(ec_sdo_request_t));

   409

    if (!req) {

   410

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

   411

        return 0;

   412

    }

   413

   414

    if (size) {

   415

        req->data = malloc(size);

   416

        if (!req->data) {

   417

            fprintf(stderr, "Failed to allocate %u bytes of SDO data"

   418

                    " memory.\n", size);

   419

            free(req);

   420

            return 0;

   421

        }

   422

    } else {

   423

        req->data = NULL;

   424

    }

   425

   426

    data.config_index = sc->index;

   427

    data.sdo_index = index;

   428

    data.sdo_subindex = subindex;

   429

    data.size = size;

   430

   431

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

   432

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

   433

                strerror(errno));

   434

        ec_sdo_request_clear(req);

   435

        free(req);

   436

        return NULL;

   437

    }

   438

   439

    req->next = NULL;

   440

    req->config = sc;

   441

    req->index = data.request_index;

   442

    req->sdo_index = data.sdo_index;

   443

    req->sdo_subindex = data.sdo_subindex;

   444

    req->data_size = size;

   445

    req->mem_size = size;

   446

   447

    ec_slave_config_add_sdo_request(sc, req);

   448

   449

    return req;

   450

}

   451

   452

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

   453

   454

void ec_slave_config_add_voe_handler(ec_slave_config_t *sc,

   455

        ec_voe_handler_t *voe)

   456

{

   457

    if (sc->first_voe_handler) {

   458

        ec_voe_handler_t *v = sc->first_voe_handler;

   459

        while (v->next) {

   460

            v = v->next;

   461

        }

   462

        v->next = voe;

   463

    } else {

   464

        sc->first_voe_handler = voe;

   465

    }

   466

}

   467

   468

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

   469

   470

ec_voe_handler_t *ecrt_slave_config_create_voe_handler(ec_slave_config_t *sc,

   471

        size_t size)

   472

{

   473

    ec_ioctl_voe_t data;

   474

    ec_voe_handler_t *voe;

   475

   476

    voe = malloc(sizeof(ec_voe_handler_t));

   477

    if (!voe) {

   478

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

   479

        return 0;

   480

    }

   481

   482

    if (size) {

   483

        voe->data = malloc(size);

   484

        if (!voe->data) {

   485

            fprintf(stderr, "Failed to allocate %u bytes of VoE data"

   486

                    " memory.\n", size);

   487

            free(voe);

   488

            return 0;

   489

        }

   490

    } else {

   491

        voe->data = NULL;

   492

    }

   493

   494

    data.config_index = sc->index;

   495

    data.size = size;

   496

   497

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

   498

        fprintf(stderr, "Failed to create VoE handler: %s\n",

   499

                strerror(errno));

   500

        ec_voe_handler_clear(voe);

   501

        free(voe);

   502

        return NULL;

   503

    }

   504

   505

    voe->next = NULL;