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

    90

int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc,

    91

        uint8_t sync_index, uint16_t pdo_index)

    92

{

    93

    ec_ioctl_config_pdo_t data;

    94

    95

    data.config_index = sc->index;

    96

    data.sync_index = sync_index;

    97

    data.index = pdo_index;

    98

    99

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

   100

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

   101

                strerror(errno));

   102

        return -1;  // FIXME

   103

    }

   104

   105

    return 0;

   106

}

   107

   108

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

   109

   110

void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc,

   111

        uint8_t sync_index)

   112

{

   113

    ec_ioctl_config_pdo_t data;

   114

   115

    data.config_index = sc->index;

   116

    data.sync_index = sync_index;

   117

   118

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

   119

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

   120

                strerror(errno));

   121

    }

   122

}

   123

   124

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

   125

   126

int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc,

   127

        uint16_t pdo_index, uint16_t entry_index, uint8_t entry_subindex,

   128

        uint8_t entry_bit_length)

   129

{

   130

    ec_ioctl_add_pdo_entry_t data;

   131

   132

    data.config_index = sc->index;

   133

    data.pdo_index = pdo_index;

   134

    data.entry_index = entry_index;

   135

    data.entry_subindex = entry_subindex;

   136

    data.entry_bit_length = entry_bit_length;

   137

   138

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

   139

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

   140

                strerror(errno));

   141

        return -1;  // FIXME

   142

    }

   143

   144

    return 0;

   145

}

   146

   147

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

   148

   149

void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc,

   150

        uint16_t pdo_index)

   151

{

   152

    ec_ioctl_config_pdo_t data;

   153

   154

    data.config_index = sc->index;

   155

    data.index = pdo_index;

   156

   157

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

   158

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

   159

                strerror(errno));

   160

    }

   161

}

   162

   163

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

   164

   165

int ecrt_slave_config_pdos(ec_slave_config_t *sc,

   166

        unsigned int n_syncs, const ec_sync_info_t syncs[])

   167

{

   168

    int ret;

   169

    unsigned int i, j, k;

   170

    const ec_sync_info_t *sync_info;

   171

    const ec_pdo_info_t *pdo_info;

   172

    const ec_pdo_entry_info_t *entry_info;

   173

   174

    if (!syncs)

   175

        return 0;

   176

   177

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

   178

        sync_info = &syncs[i];

   179

   180

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

   181

            break;

   182

   183

        if (sync_info->index >= EC_MAX_SYNC_MANAGERS) {

   184

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

   185

                    sync_info->index);

   186

            return -ENOENT;

   187

        }

   188

   189

        ret = ecrt_slave_config_sync_manager(sc, sync_info->index,

   190

                sync_info->dir, sync_info->watchdog_mode);

   191

        if (ret)

   192

            return ret;

   193

   194

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

   195

            ecrt_slave_config_pdo_assign_clear(sc, sync_info->index);

   196

   197

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

   198

                pdo_info = &sync_info->pdos[j];

   199

   200

                ret = ecrt_slave_config_pdo_assign_add(

   201

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

   202

                if (ret)

   203

                    return ret;

   204

   205

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

   206

                    ecrt_slave_config_pdo_mapping_clear(sc, pdo_info->index);

   207

   208

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

   209

                        entry_info = &pdo_info->entries[k];

   210

   211

                        ret = ecrt_slave_config_pdo_mapping_add(sc,

   212

                                pdo_info->index, entry_info->index,

   213

                                entry_info->subindex,

   214

                                entry_info->bit_length);

   215

                        if (ret)

   216

                            return ret;

   217

                    }

   218

                }

   219

            }

   220

        }

   221

    }

   222

   223

    return 0;

   224

}

   225

   226

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

   227

   228

int ecrt_slave_config_reg_pdo_entry(

   229

        ec_slave_config_t *sc,

   230

        uint16_t index,

   231

        uint8_t subindex,

   232

        ec_domain_t *domain,

   233

        unsigned int *bit_position

   234

        )

   235

{

   236

    ec_ioctl_reg_pdo_entry_t data;

   237

    int ret;

   238

   239

    data.config_index = sc->index;

   240

    data.entry_index = index;

   241

    data.entry_subindex = subindex;

   242

    data.domain_index = domain->index;

   243

   244

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

   245

    if (ret == -1) {

   246

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

   247

                strerror(errno));

   248

        return -2; // FIXME

   249

    }

   250

   251

    if (bit_position) {

   252

        *bit_position = data.bit_position;

   253

    } else {

   254

        if (data.bit_position) {

   255

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

   256

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

   257

                    sc->alias, sc->position);

   258

            return -3; // FIXME

   259

        }

   260

    }

   261

   262

    return ret;

   263

}

   264

   265

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

   266

   267

void ecrt_slave_config_dc(ec_slave_config_t *sc, uint16_t assign_activate,

   268

        uint32_t sync0_cycle_time, uint32_t sync0_shift_time,

   269

        uint32_t sync1_cycle_time, uint32_t sync1_shift_time)

   270

{

   271

    ec_ioctl_config_t data;

   272

   273

    data.config_index = sc->index;

   274

    data.dc_assign_activate = assign_activate;

   275

    data.dc_sync[0].cycle_time = sync0_cycle_time;

   276

    data.dc_sync[0].shift_time = sync0_shift_time;

   277

    data.dc_sync[1].cycle_time = sync1_cycle_time;

   278

    data.dc_sync[1].shift_time = sync1_shift_time;

   279

   280

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

   281

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

   282

    }

   283

}

   284

   285

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

   286

   287

int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index,

   288

        uint8_t subindex, const uint8_t *sdo_data, size_t size)

   289

{

   290

    ec_ioctl_sc_sdo_t data;

   291

   292

    data.config_index = sc->index;

   293

    data.index = index;

   294

    data.subindex = subindex;

   295

    data.data = sdo_data;

   296

    data.size = size;

   297

    data.complete_access = 0;

   298

   299

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

   300

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

   301

        return -1; // FIXME

   302

    }

   303

   304

    return 0;

   305

}

   306

   307

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

   308

   309

int ecrt_slave_config_complete_sdo(ec_slave_config_t *sc, uint16_t index,

   310

        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 = 0;

   317

    data.data = sdo_data;

   318

    data.size = size;

   319

    data.complete_access = 1;

   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_sdo8(ec_slave_config_t *sc, uint16_t index,

   332

        uint8_t subindex, uint8_t value)

   333

{

   334

    uint8_t data[1];

   335

   336

    EC_WRITE_U8(data, value);

   337

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

   338

}

   339

   340

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

   341

   342

int ecrt_slave_config_sdo16(ec_slave_config_t *sc, uint16_t index,

   343

        uint8_t subindex, uint16_t value)

   344

{

   345

    uint8_t data[2];

   346

   347

    EC_WRITE_U16(data, value);

   348

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

   349

}

   350

   351

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

   352

   353

int ecrt_slave_config_sdo32(ec_slave_config_t *sc, uint16_t index,

   354

        uint8_t subindex, uint32_t value)

   355

{

   356

    uint8_t data[4];

   357

   358

    EC_WRITE_U32(data, value);

   359

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

   360

}

   361

   362

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

   363

   364

ec_sdo_request_t *ecrt_slave_config_create_sdo_request(ec_slave_config_t *sc,

   365

        uint16_t index, uint8_t subindex, size_t size)

   366

{

   367

    ec_ioctl_sdo_request_t data;

   368

    ec_sdo_request_t *req;

   369

   370

    req = malloc(sizeof(ec_sdo_request_t));

   371

    if (!req) {

   372

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

   373

        return 0;

   374

    }

   375

   376

    if (size) {

   377

        req->data = malloc(size);

   378

        if (!req->data) {

   379

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

   380

                    " memory.\n", size);

   381

            free(req);

   382

            return 0;

   383

        }

   384

    } else {

   385

        req->data = NULL;

   386

    }

   387

   388

    data.config_index = sc->index;

   389

    data.sdo_index = index;

   390

    data.sdo_subindex = subindex;

   391

    data.size = size;

   392

   393

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

   394

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

   395

                strerror(errno));

   396

        if (req->data)

   397

            free(req->data);

   398

        free(req);

   399

        return NULL; 

   400

    }

   401

   402

    req->config = sc;

   403

    req->index = data.request_index;

   404

    req->sdo_index = data.sdo_index;

   405

    req->sdo_subindex = data.sdo_subindex;

   406

    req->data_size = size;

   407

    req->mem_size = size;

   408

    return req;

   409

}

   410

   411

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

   412

   413

ec_voe_handler_t *ecrt_slave_config_create_voe_handler(ec_slave_config_t *sc,

   414

        size_t size)

   415

{

   416

    ec_ioctl_voe_t data;

   417

    ec_voe_handler_t *voe;

   418

    unsigned int index;

   419

   420

    voe = malloc(sizeof(ec_voe_handler_t));

   421

    if (!voe) {

   422

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

   423

        return 0;

   424

    }

   425

   426

    if (size) {

   427

        voe->data = malloc(size);

   428

        if (!voe->data) {

   429

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

   430

                    " memory.\n", size);

   431

            free(voe);

   432

            return 0;

   433

        }

   434

    } else {

   435

        voe->data = NULL;

   436

    }

   437

   438

    data.config_index = sc->index;

   439

    data.size = size;

   440

   441

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

   442

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

   443

                strerror(errno));

   444

        if (voe->data)

   445

            free(voe->data);

   446

        free(voe);

   447

        return NULL; 

   448

    }

   449

   450

    voe->config = sc;

   451

    voe->index = data.voe_index;

   452

    voe->data_size = size;

   453

    voe->mem_size = size;

   454

    return voe;

   455

}

   456

   457

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

   458

   459

void ecrt_slave_config_state(const ec_slave_config_t *sc,

   460

        ec_slave_config_state_t *state)

   461

{

   462

    ec_ioctl_sc_state_t data;

   463

   464

    data.config_index = sc->index;

   465

    data.state = state;

   466

   467

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

   468

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

   469

                strerror(errno));

   470

    }

   471

}

   472

   473

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