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

int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index,

    47

        ec_direction_t dir, ec_watchdog_mode_t watchdog_mode)

    48

{

    49

    ec_ioctl_config_t data;

    50

    unsigned int i;

    51

    52

    if (sync_index >= EC_MAX_SYNC_MANAGERS)

    53

        return -ENOENT;

    54

    55

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

    56

    data.config_index = sc->index;

    57

    data.syncs[sync_index].dir = dir;

    58

    data.syncs[sync_index].watchdog_mode = watchdog_mode;

    59

    data.syncs[sync_index].config_this = 1;

    60

    61

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

    62

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

    63

                strerror(errno));

    64

        return -1; // FIXME

    65

    }

    66

    67

    return 0;

    68

}

    69

    70

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

    71

    72

void ecrt_slave_config_watchdog(ec_slave_config_t *sc,

    73

        uint16_t divider, uint16_t intervals)

    74

{

    75

    ec_ioctl_config_t data;

    76

    77

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

    78

    data.config_index = sc->index;

    79

    data.watchdog_divider = divider;

    80

    data.watchdog_intervals = intervals;

    81

    82

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

    83

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

    84

                strerror(errno));

    85

    }

    86

}

    87

    88

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

    89

   107

int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc,

   108

        uint8_t sync_index, uint16_t pdo_index)

   109

{

   110

    ec_ioctl_config_pdo_t data;

   111

   112

    data.config_index = sc->index;

   113

    data.sync_index = sync_index;

   114

    data.index = pdo_index;

   115

   116

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

   117

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

   118

                strerror(errno));

   119

        return -1;  // FIXME

   120

    }

   121

   122

    return 0;

   123

}

   124

   125

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

   126

   127

void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc,

   128

        uint8_t sync_index)

   129

{

   130

    ec_ioctl_config_pdo_t data;

   131

   132

    data.config_index = sc->index;

   133

    data.sync_index = sync_index;

   134

   135

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

   136

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

   137

                strerror(errno));

   138

    }

   139

}

   140

   141

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

   142

   143

int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc,

   144

        uint16_t pdo_index, uint16_t entry_index, uint8_t entry_subindex,

   145

        uint8_t entry_bit_length)

   146

{

   147

    ec_ioctl_add_pdo_entry_t data;

   148

   149

    data.config_index = sc->index;

   150

    data.pdo_index = pdo_index;

   151

    data.entry_index = entry_index;

   152

    data.entry_subindex = entry_subindex;

   153

    data.entry_bit_length = entry_bit_length;

   154

   155

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

   156

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

   157

                strerror(errno));

   158

        return -1;  // FIXME

   159

    }

   160

   161

    return 0;

   162

}

   163

   164

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

   165

   166

void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc,

   167

        uint16_t pdo_index)

   168

{

   169

    ec_ioctl_config_pdo_t data;

   170

   171

    data.config_index = sc->index;

   172

    data.index = pdo_index;

   173

   174

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

   175

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

   176

                strerror(errno));

   177

    }

   178

}

   179

   180

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

   181

   182

int ecrt_slave_config_pdos(ec_slave_config_t *sc,

   183

        unsigned int n_syncs, const ec_sync_info_t syncs[])

   184

{

   185

    int ret;

   186

    unsigned int i, j, k;

   187

    const ec_sync_info_t *sync_info;

   188

    const ec_pdo_info_t *pdo_info;

   189

    const ec_pdo_entry_info_t *entry_info;

   190

   191

    if (!syncs)

   192

        return 0;

   193

   194

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

   195

        sync_info = &syncs[i];

   196

   197

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

   198

            break;

   199

   200

        if (sync_info->index >= EC_MAX_SYNC_MANAGERS) {

   201

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

   202

                    sync_info->index);

   203

            return -ENOENT;

   204

        }

   205

   206

        ret = ecrt_slave_config_sync_manager(sc, sync_info->index,

   207

                sync_info->dir, sync_info->watchdog_mode);

   208

        if (ret)

   209

            return ret;

   210

   211

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

   212

            ecrt_slave_config_pdo_assign_clear(sc, sync_info->index);

   213

   214

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

   215

                pdo_info = &sync_info->pdos[j];

   216

   217

                ret = ecrt_slave_config_pdo_assign_add(

   218

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

   219

                if (ret)

   220

                    return ret;

   221

   222

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

   223

                    ecrt_slave_config_pdo_mapping_clear(sc, pdo_info->index);

   224

   225

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

   226

                        entry_info = &pdo_info->entries[k];

   227

   228

                        ret = ecrt_slave_config_pdo_mapping_add(sc,

   229

                                pdo_info->index, entry_info->index,

   230

                                entry_info->subindex,

   231

                                entry_info->bit_length);

   232

                        if (ret)

   233

                            return ret;

   234

                    }

   235

                }

   236

            }

   237

        }

   238

    }

   239

   240

    return 0;

   241

}

   242

   243

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

   244

   245

int ecrt_slave_config_reg_pdo_entry(

   246

        ec_slave_config_t *sc,

   247

        uint16_t index,

   248

        uint8_t subindex,

   249

        ec_domain_t *domain,

   250

        unsigned int *bit_position

   251

        )

   252

{

   253

    ec_ioctl_reg_pdo_entry_t data;

   254

    int ret;

   255

   256

    data.config_index = sc->index;

   257

    data.entry_index = index;

   258

    data.entry_subindex = subindex;

   259

    data.domain_index = domain->index;

   260

   261

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

   262

    if (ret == -1) {

   263

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

   264

                strerror(errno));

   265

        return -2; // FIXME

   266

    }

   267

   268

    if (bit_position) {

   269

        *bit_position = data.bit_position;

   270

    } else {

   271

        if (data.bit_position) {

   272

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

   273

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

   274

                    sc->alias, sc->position);

   275

            return -3; // FIXME

   276

        }

   277

    }

   278

   279

    return ret;

   280

}

   281

   282

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

   283

   284

void ecrt_slave_config_dc(ec_slave_config_t *sc, uint16_t assign_activate,

   285

        uint32_t sync0_cycle_time, uint32_t sync0_shift_time,

   286

        uint32_t sync1_cycle_time, uint32_t sync1_shift_time)

   287

{

   288

    ec_ioctl_config_t data;

   289

   290

    data.config_index = sc->index;

   291

    data.dc_assign_activate = assign_activate;

   292

    data.dc_sync[0].cycle_time = sync0_cycle_time;

   293

    data.dc_sync[0].shift_time = sync0_shift_time;

   294

    data.dc_sync[1].cycle_time = sync1_cycle_time;

   295

    data.dc_sync[1].shift_time = sync1_shift_time;

   296

   297

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

   298

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

   299

    }

   300

}

   301

   302

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

   303

   304

int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index,

   305

        uint8_t subindex, const uint8_t *sdo_data, size_t size)

   306

{

   307

    ec_ioctl_sc_sdo_t data;

   308

   309

    data.config_index = sc->index;

   310

    data.index = index;

   311

    data.subindex = subindex;

   312

    data.data = sdo_data;

   313

    data.size = size;

   314

    data.complete_access = 0;

   315

   316

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

   317

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

   318

        return -1; // FIXME

   319

    }

   320

   321

    return 0;

   322

}

   323

   324

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

   325

   326

int ecrt_slave_config_complete_sdo(ec_slave_config_t *sc, uint16_t index,

   327

        const uint8_t *sdo_data, size_t size)

   328

{

   329

    ec_ioctl_sc_sdo_t data;

   330

   331

    data.config_index = sc->index;

   332

    data.index = index;

   333

    data.subindex = 0;

   334

    data.data = sdo_data;

   335

    data.size = size;

   336

    data.complete_access = 1;

   337

   338

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

   339

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

   340

        return -1; // FIXME

   341

    }

   342

   343

    return 0;

   344

}

   345

   346

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

   347

   348

int ecrt_slave_config_sdo8(ec_slave_config_t *sc, uint16_t index,

   349

        uint8_t subindex, uint8_t value)

   350

{

   351

    uint8_t data[1];

   352

   353

    EC_WRITE_U8(data, value);

   354

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

   355

}

   356

   357

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

   358

   359

int ecrt_slave_config_sdo16(ec_slave_config_t *sc, uint16_t index,

   360

        uint8_t subindex, uint16_t value)

   361

{

   362

    uint8_t data[2];

   363

   364

    EC_WRITE_U16(data, value);

   365

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

   366

}

   367

   368

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

   369

   370

int ecrt_slave_config_sdo32(ec_slave_config_t *sc, uint16_t index,

   371

        uint8_t subindex, uint32_t value)

   372

{

   373

    uint8_t data[4];

   374

   375

    EC_WRITE_U32(data, value);

   376

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

   377

}

   378

   379

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

   380

   381

ec_sdo_request_t *ecrt_slave_config_create_sdo_request(ec_slave_config_t *sc,

   382

        uint16_t index, uint8_t subindex, size_t size)

   383

{

   384

    ec_ioctl_sdo_request_t data;

   385

    ec_sdo_request_t *req;

   386

   387

    req = malloc(sizeof(ec_sdo_request_t));

   388

    if (!req) {

   389

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

   390

        return 0;

   391

    }

   392

   393

    if (size) {

   394

        req->data = malloc(size);

   395

        if (!req->data) {

   396

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

   397

                    " memory.\n", size);

   398

            free(req);

   399

            return 0;

   400

        }

   401

    } else {

   402

        req->data = NULL;

   403

    }

   404

   405

    data.config_index = sc->index;

   406

    data.sdo_index = index;

   407

    data.sdo_subindex = subindex;

   408

    data.size = size;

   409

   410

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

   411

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

   412

                strerror(errno));

   413

        if (req->data)

   414

            free(req->data);

   415

        free(req);

   416

        return NULL; 

   417

    }

   418

   419

    req->config = sc;

   420

    req->index = data.request_index;

   421

    req->sdo_index = data.sdo_index;

   422

    req->sdo_subindex = data.sdo_subindex;

   423

    req->data_size = size;

   424

    req->mem_size = size;

   425

    return req;

   426

}

   427

   428

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

   429

   430

ec_voe_handler_t *ecrt_slave_config_create_voe_handler(ec_slave_config_t *sc,

   431

        size_t size)

   432

{

   433

    ec_ioctl_voe_t data;

   434

    ec_voe_handler_t *voe;

   435

    unsigned int index;

   436

   437

    voe = malloc(sizeof(ec_voe_handler_t));

   438

    if (!voe) {

   439

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

   440

        return 0;

   441

    }

   442

   443

    if (size) {

   444

        voe->data = malloc(size);

   445

        if (!voe->data) {

   446

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

   447

                    " memory.\n", size);

   448

            free(voe);

   449

            return 0;

   450

        }

   451

    } else {

   452

        voe->data = NULL;

   453

    }

   454

   455

    data.config_index = sc->index;

   456

    data.size = size;

   457

   458

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

   459

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

   460

                strerror(errno));

   461

        if (voe->data)

   462

            free(voe->data);

   463

        free(voe);

   464

        return NULL; 

   465

    }

   466

   467

    voe->config = sc;

   468

    voe->index = data.voe_index;

   469

    voe->data_size = size;

   470

    voe->mem_size = size;

   471

    return voe;

   472

}

   473

   474

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

   475

   476

void ecrt_slave_config_state(const ec_slave_config_t *sc,

   477

        ec_slave_config_state_t *state)

   478

{

   479

    ec_ioctl_sc_state_t data;

   480

   481

    data.config_index = sc->index;

   482

    data.state = state;

   483

   484

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

   485

        fprintf(stderr, "Failed to get slave configuration state: %s\n",

   486

                strerror(errno));

   487

    }

   488

}

   489

   490

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

   491

   492

int ecrt_slave_config_idn(ec_slave_config_t *sc, uint16_t idn,

   493

        const uint8_t *data, size_t size)

   494

{

   495

    ec_ioctl_sc_idn_t io;

   496

   497

    io.config_index = sc->index;

   498

    io.idn = idn;

   499

    io.data = data;

   500

    io.size = size;

   501

   502

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

   503

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

   504

        return -1; // FIXME

   505

    }

   506

   507

    return 0;

   508

}