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)

    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

    59

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

    60

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

    61

                strerror(errno));

    62

        return -1; // FIXME

    63

    }

    64

    65

    return 0;

    66

}

    67

    68

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

    69

    70

int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc,

    71

        uint8_t sync_index, uint16_t pdo_index)

    72

{

    73

    ec_ioctl_config_pdo_t data;

    74

    75

    data.config_index = sc->index;

    76

    data.sync_index = sync_index;

    77

    data.index = pdo_index;

    78

    79

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

    80

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

    81

                strerror(errno));

    82

        return -1;  // FIXME

    83

    }

    84

    85

    return 0;

    86

}

    87

    88

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

    89

    90

void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc,

    91

        uint8_t sync_index)

    92

{

    93

    ec_ioctl_config_pdo_t data;

    94

    95

    data.config_index = sc->index;

    96

    data.sync_index = sync_index;

    97

    98

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

    99

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

   100

                strerror(errno));

   101

    }

   102

}

   103

   104

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

   105

   106

int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc,

   107

        uint16_t pdo_index, uint16_t entry_index, uint8_t entry_subindex,

   108

        uint8_t entry_bit_length)

   109

{

   110

    ec_ioctl_add_pdo_entry_t data;

   111

   112

    data.config_index = sc->index;

   113

    data.pdo_index = pdo_index;

   114

    data.entry_index = entry_index;

   115

    data.entry_subindex = entry_subindex;

   116

    data.entry_bit_length = entry_bit_length;

   117

   118

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

   119

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

   120

                strerror(errno));

   121

        return -1;  // FIXME

   122

    }

   123

   124

    return 0;

   125

}

   126

   127

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

   128

   129

void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc,

   130

        uint16_t pdo_index)

   131

{

   132

    ec_ioctl_config_pdo_t data;

   133

   134

    data.config_index = sc->index;

   135

    data.index = pdo_index;

   136

   137

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

   138

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

   139

                strerror(errno));

   140

    }

   141

}

   142

   143

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

   144

   145

int ecrt_slave_config_pdos(ec_slave_config_t *sc,

   146

        unsigned int n_syncs, const ec_sync_info_t syncs[])

   147

{

   148

    int ret;

   149

    unsigned int i, j, k;

   150

    const ec_sync_info_t *sync_info;

   151

    const ec_pdo_info_t *pdo_info;

   152

    const ec_pdo_entry_info_t *entry_info;

   153

   154

    if (!syncs)

   155

        return 0;

   156

   157

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

   158

        sync_info = &syncs[i];

   159

   160

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

   161

            break;

   162

   163

        if (sync_info->index >= EC_MAX_SYNC_MANAGERS) {

   164

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

   165

                    sync_info->index);

   166

            return -ENOENT;

   167

        }

   168

   169

        ret = ecrt_slave_config_sync_manager(

   170

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

   171

        if (ret)

   172

            return ret;

   173

   174

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

   175

            ecrt_slave_config_pdo_assign_clear(sc, sync_info->index);

   176

   177

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

   178

                pdo_info = &sync_info->pdos[j];

   179

   180

                ret = ecrt_slave_config_pdo_assign_add(

   181

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

   182

                if (ret)

   183

                    return ret;

   184

   185

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

   186

                    ecrt_slave_config_pdo_mapping_clear(sc, pdo_info->index);

   187

   188

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

   189

                        entry_info = &pdo_info->entries[k];

   190

   191

                        ret = ecrt_slave_config_pdo_mapping_add(sc,

   192

                                pdo_info->index, entry_info->index,

   193

                                entry_info->subindex,

   194

                                entry_info->bit_length);

   195

                        if (ret)

   196

                            return ret;

   197

                    }

   198

                }

   199

            }

   200

        }

   201

    }

   202

   203

    return 0;

   204

}

   205

   206

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

   207

   208

int ecrt_slave_config_reg_pdo_entry(

   209

        ec_slave_config_t *sc,

   210

        uint16_t index,

   211

        uint8_t subindex,

   212

        ec_domain_t *domain,

   213

        unsigned int *bit_position

   214

        )

   215

{

   216

    ec_ioctl_reg_pdo_entry_t data;

   217

    int ret;

   218

   219

    data.config_index = sc->index;

   220

    data.entry_index = index;

   221

    data.entry_subindex = subindex;

   222

    data.domain_index = domain->index;

   223

   224

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

   225

    if (ret == -1) {

   226

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

   227

                strerror(errno));

   228

        return -2; // FIXME

   229

    }

   230

   231

    if (bit_position) {

   232

        *bit_position = data.bit_position;

   233

    } else {

   234

        if (data.bit_position) {

   235

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

   236

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

   237

                    sc->alias, sc->position);

   238

            return -3; // FIXME

   239

        }

   240

    }

   241

   242

    return ret;

   243

}

   244

   245

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

   246

   247

void ecrt_slave_config_dc_assign_activate(ec_slave_config_t *sc,

   248

        uint16_t assign_activate)

   249

{

   250

    ec_ioctl_sc_dc_t data;

   251

   252

    data.config_index = sc->index;

   253

    data.assign_activate = assign_activate;

   254

   255

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

   256

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

   257

    }

   258

}

   259

   260

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

   261

   262

void ecrt_slave_config_dc_sync_cycle_times(ec_slave_config_t *sc,

   263

        uint32_t sync0_cycle_time, uint32_t sync1_cycle_time)

   264

{

   265

    ec_ioctl_sc_dc_t data;

   266

   267

    data.config_index = sc->index;

   268

	data.cycle[0] = sync0_cycle_time;

   269

	data.cycle[1] = sync1_cycle_time;

   270

   271

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

   272

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

   273

    }

   274

}

   275

   276

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

   277

   278

void ecrt_slave_config_dc_sync_shift_times(ec_slave_config_t *sc,

   279

        uint32_t sync0_shift_time, uint32_t sync1_shift_time)

   280

{

   281

    ec_ioctl_sc_dc_t data;

   282

   283

    data.config_index = sc->index;

   284

	data.shift[0] = sync0_shift_time;

   285

	data.shift[1] = sync1_shift_time;

   286

   287

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

   288

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

   289

    }

   290

}

   291

   292

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

   293

   294

int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index,

   295

        uint8_t subindex, const uint8_t *sdo_data, size_t size)

   296

{

   297

    ec_ioctl_sc_sdo_t data;

   298

   299

    data.config_index = sc->index;

   300

    data.index = index;

   301

    data.subindex = subindex;

   302

    data.data = sdo_data;

   303

    data.size = size;

   304

   305

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

   306

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

   307

        return -1; // FIXME

   308

    }

   309

   310

    return 0;

   311

}

   312

   313

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

   314

   315

int ecrt_slave_config_sdo8(ec_slave_config_t *sc, uint16_t index,

   316

        uint8_t subindex, uint8_t value)

   317

{

   318

    uint8_t data[1];

   319

   320

    EC_WRITE_U8(data, value);

   321

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

   322

}

   323

   324

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

   325

   326

int ecrt_slave_config_sdo16(ec_slave_config_t *sc, uint16_t index,

   327

        uint8_t subindex, uint16_t value)

   328

{

   329

    uint8_t data[2];

   330

   331

    EC_WRITE_U16(data, value);

   332

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

   333

}

   334

   335

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

   336

   337

int ecrt_slave_config_sdo32(ec_slave_config_t *sc, uint16_t index,

   338

        uint8_t subindex, uint32_t value)

   339

{

   340

    uint8_t data[4];

   341

   342

    EC_WRITE_U32(data, value);

   343

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

   344

}

   345

   346

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

   347

   348

ec_sdo_request_t *ecrt_slave_config_create_sdo_request(ec_slave_config_t *sc,

   349

        uint16_t index, uint8_t subindex, size_t size)

   350

{

   351

    ec_ioctl_sdo_request_t data;

   352

    ec_sdo_request_t *req;

   353

   354

    req = malloc(sizeof(ec_sdo_request_t));

   355

    if (!req) {

   356

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

   357

        return 0;

   358

    }

   359

   360

    if (size) {

   361

        req->data = malloc(size);

   362

        if (!req->data) {

   363

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

   364

                    " memory.\n", size);

   365

            free(req);

   366

            return 0;

   367

        }

   368

    } else {

   369

        req->data = NULL;

   370

    }

   371

   372

    data.config_index = sc->index;

   373

    data.sdo_index = index;

   374

    data.sdo_subindex = subindex;

   375

    data.size = size;

   376

   377

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

   378

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

   379

                strerror(errno));

   380

        if (req->data)

   381

            free(req->data);

   382

        free(req);

   383

        return NULL; 

   384

    }

   385

   386

    req->config = sc;

   387

    req->index = data.request_index;

   388

    req->sdo_index = data.sdo_index;

   389

    req->sdo_subindex = data.sdo_subindex;

   390

    req->data_size = size;

   391

    req->mem_size = size;

   392

    return req;

   393

}

   394

   395

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

   396

   397

ec_voe_handler_t *ecrt_slave_config_create_voe_handler(ec_slave_config_t *sc,

   398

        size_t size)

   399

{

   400

    ec_ioctl_voe_t data;

   401

    ec_voe_handler_t *voe;

   402

    unsigned int index;

   403

   404

    voe = malloc(sizeof(ec_voe_handler_t));

   405

    if (!voe) {

   406

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

   407

        return 0;

   408

    }

   409

   410

    if (size) {

   411

        voe->data = malloc(size);

   412

        if (!voe->data) {

   413

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

   414

                    " memory.\n", size);

   415

            free(voe);

   416

            return 0;

   417

        }

   418

    } else {

   419

        voe->data = NULL;

   420

    }

   421

   422

    data.config_index = sc->index;

   423

    data.size = size;

   424

   425

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

   426

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

   427

                strerror(errno));

   428

        if (voe->data)

   429

            free(voe->data);

   430

        free(voe);

   431

        return NULL; 

   432

    }

   433

   434

    voe->config = sc;

   435

    voe->index = data.voe_index;

   436

    voe->data_size = size;

   437

    voe->mem_size = size;

   438

    return voe;

   439

}

   440

   441

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

   442

   443

void ecrt_slave_config_state(const ec_slave_config_t *sc,

   444

        ec_slave_config_state_t *state)

   445

{

   446

    ec_ioctl_sc_state_t data;

   447

   448

    data.config_index = sc->index;

   449

    data.state = state;

   450

   451

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

   452

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

   453

                strerror(errno));

   454

    }

   455

}

   456

   457

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