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

 *  Using the EtherCAT technology and brand is permitted in compliance with

    24

 *  the industrial property and similar rights of Beckhoff Automation GmbH.

    25

 *

    26

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

    27

    28

#include <stdlib.h>

    29

#include <sys/ioctl.h>

    30

#include <stdio.h>

    31

#include <errno.h>

    32

#include <string.h>

    33

    34

#include "slave_config.h"

    35

#include "domain.h"

    36

#include "sdo_request.h"

    37

#include "voe_handler.h"

    38

#include "master.h"

    39

#include "master/ioctl.h"

    40

    41

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

    42

    43

int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index,

    44

        ec_direction_t dir)

    45

{

    46

    ec_ioctl_config_t data;

    47

    unsigned int i;

    48

    49

    if (sync_index >= EC_MAX_SYNC_MANAGERS)

    50

        return -ENOENT;

    51

    52

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

    53

    data.config_index = sc->index;

    54

    data.syncs[sync_index].dir = dir;

    55

    56

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

    57

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

    58

                strerror(errno));

    59

        return -1; // FIXME

    60

    }

    61

    62

    return 0;

    63

}

    64

    65

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

    66

    67

int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc,

    68

        uint8_t sync_index, uint16_t pdo_index)

    69

{

    70

    ec_ioctl_config_pdo_t data;

    71

    72

    data.config_index = sc->index;

    73

    data.sync_index = sync_index;

    74

    data.index = pdo_index;

    75

    76

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

    77

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

    78

                strerror(errno));

    79

        return -1;  // FIXME

    80

    }

    81

    82

    return 0;

    83

}

    84

    85

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

    86

    87

void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc,

    88

        uint8_t sync_index)

    89

{

    90

    ec_ioctl_config_pdo_t data;

    91

    92

    data.config_index = sc->index;

    93

    data.sync_index = sync_index;

    94

    95

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

    96

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

    97

                strerror(errno));

    98

    }

    99

}

   100

   101

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

   102

   103

int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc,

   104

        uint16_t pdo_index, uint16_t entry_index, uint8_t entry_subindex,

   105

        uint8_t entry_bit_length)

   106

{

   107

    ec_ioctl_add_pdo_entry_t data;

   108

   109

    data.config_index = sc->index;

   110

    data.pdo_index = pdo_index;

   111

    data.entry_index = entry_index;

   112

    data.entry_subindex = entry_subindex;

   113

    data.entry_bit_length = entry_bit_length;

   114

   115

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

   116

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

   117

                strerror(errno));

   118

        return -1;  // FIXME

   119

    }

   120

   121

    return 0;

   122

}

   123

   124

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

   125

   126

void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc,

   127

        uint16_t pdo_index)

   128

{

   129

    ec_ioctl_config_pdo_t data;

   130

   131

    data.config_index = sc->index;

   132

    data.index = pdo_index;

   133

   134

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

   135

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

   136

                strerror(errno));

   137

    }

   138

}

   139

   140

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

   141

   142

int ecrt_slave_config_pdos(ec_slave_config_t *sc,

   143

        unsigned int n_syncs, const ec_sync_info_t syncs[])

   144

{

   145

    int ret;

   146

    unsigned int i, j, k;

   147

    const ec_sync_info_t *sync_info;

   148

    const ec_pdo_info_t *pdo_info;

   149

    const ec_pdo_entry_info_t *entry_info;

   150

   151

    if (!syncs)

   152

        return 0;

   153

   154

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

   155

        sync_info = &syncs[i];

   156

   157

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

   158

            break;

   159

   160

        if (sync_info->index >= EC_MAX_SYNC_MANAGERS) {

   161

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

   162

                    sync_info->index);

   163

            return -ENOENT;

   164

        }

   165

   166

        ret = ecrt_slave_config_sync_manager(

   167

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

   168

        if (ret)

   169

            return ret;

   170

   171

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

   172

            ecrt_slave_config_pdo_assign_clear(sc, sync_info->index);

   173

   174

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

   175

                pdo_info = &sync_info->pdos[j];

   176

   177

                ret = ecrt_slave_config_pdo_assign_add(

   178

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

   179

                if (ret)

   180

                    return ret;

   181

   182

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

   183

                    ecrt_slave_config_pdo_mapping_clear(sc, pdo_info->index);

   184

   185

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

   186

                        entry_info = &pdo_info->entries[k];

   187

   188

                        ret = ecrt_slave_config_pdo_mapping_add(sc,

   189

                                pdo_info->index, entry_info->index,

   190

                                entry_info->subindex,

   191

                                entry_info->bit_length);

   192

                        if (ret)

   193

                            return ret;

   194

                    }

   195

                }

   196

            }

   197

        }

   198

    }

   199

   200

    return 0;

   201

}

   202

   203

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

   204

   205

int ecrt_slave_config_reg_pdo_entry(

   206

        ec_slave_config_t *sc,

   207

        uint16_t index,

   208

        uint8_t subindex,

   209

        ec_domain_t *domain,

   210

        unsigned int *bit_position

   211

        )

   212

{

   213

    ec_ioctl_reg_pdo_entry_t data;

   214

    int ret;

   215

   216

    data.config_index = sc->index;

   217

    data.entry_index = index;

   218

    data.entry_subindex = subindex;

   219

    data.domain_index = domain->index;

   220

   221

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

   222

    if (ret == -1) {

   223

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

   224

                strerror(errno));

   225

        return -2; // FIXME

   226

    }

   227

   228

    if (bit_position) {

   229

        *bit_position = data.bit_position;

   230

    } else {

   231

        if (data.bit_position) {

   232

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

   233

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

   234

                    sc->alias, sc->position);

   235

            return -3; // FIXME

   236

        }

   237

    }

   238

   239

    return ret;

   240

}

   241

   242

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

   243

   244

int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index,

   245

        uint8_t subindex, const uint8_t *sdo_data, size_t size)

   246

{

   247

    ec_ioctl_sc_sdo_t data;

   248

   249

    data.config_index = sc->index;

   250

    data.index = index;

   251

    data.subindex = subindex;

   252

    data.data = sdo_data;

   253

    data.size = size;

   254

   255

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

   256

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

   257

        return -1; // FIXME

   258

    }

   259

   260

    return 0;

   261

}

   262

   263

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

   264

   265

int ecrt_slave_config_sdo8(ec_slave_config_t *sc, uint16_t index,

   266

        uint8_t subindex, uint8_t value)

   267

{

   268

    uint8_t data[1];

   269

   270

    EC_WRITE_U8(data, value);

   271

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

   272

}

   273

   274

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

   275

   276

int ecrt_slave_config_sdo16(ec_slave_config_t *sc, uint16_t index,

   277

        uint8_t subindex, uint16_t value)

   278

{

   279

    uint8_t data[2];

   280

   281

    EC_WRITE_U16(data, value);

   282

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

   283

}

   284

   285

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

   286

   287

int ecrt_slave_config_sdo32(ec_slave_config_t *sc, uint16_t index,

   288

        uint8_t subindex, uint32_t value)

   289

{

   290

    uint8_t data[4];

   291

   292

    EC_WRITE_U32(data, value);

   293

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

   294

}

   295

   296

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

   297

   298

ec_sdo_request_t *ecrt_slave_config_create_sdo_request(ec_slave_config_t *sc,

   299

        uint16_t index, uint8_t subindex, size_t size)

   300

{

   301

    ec_ioctl_sdo_request_t data;

   302

    ec_sdo_request_t *req;

   303

   304

    req = malloc(sizeof(ec_sdo_request_t));

   305

    if (!req) {

   306

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

   307

        return 0;

   308

    }

   309

   310

    if (size) {

   311

        req->data = malloc(size);

   312

        if (!req->data) {

   313

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

   314

                    " memory.\n", size);

   315

            free(req);

   316

            return 0;

   317

        }

   318

    } else {

   319

        req->data = NULL;

   320

    }

   321

   322

    data.config_index = sc->index;

   323

    data.sdo_index = index;

   324

    data.sdo_subindex = subindex;

   325

    data.size = size;

   326

   327

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

   328

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

   329

                strerror(errno));

   330

        if (req->data)

   331

            free(req->data);

   332

        free(req);

   333

        return NULL; 

   334

    }

   335

   336

    req->config = sc;

   337

    req->index = data.request_index;

   338

    req->sdo_index = data.sdo_index;

   339

    req->sdo_subindex = data.sdo_subindex;

   340

    req->data_size = size;

   341

    req->mem_size = size;

   342

    return req;

   343

}

   344

   345

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

   346

   347

ec_voe_handler_t *ecrt_slave_config_create_voe_handler(ec_slave_config_t *sc,

   348

        size_t size)

   349

{

   350

    ec_ioctl_voe_t data;

   351

    ec_voe_handler_t *voe;

   352

    unsigned int index;

   353

   354

    voe = malloc(sizeof(ec_voe_handler_t));

   355

    if (!voe) {

   356

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

   357

        return 0;

   358

    }

   359

   360

    if (size) {

   361

        voe->data = malloc(size);

   362

        if (!voe->data) {

   363

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

   364

                    " memory.\n", size);

   365

            free(voe);

   366

            return 0;

   367

        }

   368

    } else {

   369

        voe->data = NULL;

   370

    }

   371

   372

    data.config_index = sc->index;

   373

    data.size = size;

   374

   375

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

   376

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

   377

                strerror(errno));

   378

        if (voe->data)

   379

            free(voe->data);

   380

        free(voe);

   381

        return NULL; 

   382

    }

   383

   384

    voe->config = sc;

   385

    voe->index = data.voe_index;

   386

    voe->data_size = size;

   387

    voe->mem_size = size;

   388

    return voe;

   389

}

   390

   391

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

   392

   393

void ecrt_slave_config_state(const ec_slave_config_t *sc,

   394

        ec_slave_config_state_t *state)

   395

{

   396

    ec_ioctl_sc_state_t data;

   397

   398

    data.config_index = sc->index;

   399

    data.state = state;

   400

   401

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

   402

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

   403

                strerror(errno));

   404

    }

   405

}

   406

   407

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