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 "voe_handler.h"

    37

#include "master.h"

    38

#include "master/ioctl.h"

    39

    40

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

    41

    42

int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index,

    43

        ec_direction_t dir)

    44

{

    45

    ec_ioctl_config_t data;

    46

    unsigned int i;

    47

    48

    if (sync_index >= EC_MAX_SYNC_MANAGERS)

    49

        return -ENOENT;

    50

    51

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

    52

    data.config_index = sc->index;

    53

    data.syncs[sync_index].dir = dir;

    54

    55

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

    56

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

    57

                strerror(errno));

    58

        return -1; // FIXME

    59

    }

    60

    61

    return 0;

    62

}

    63

    64

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

    65

    66

int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc,

    67

        uint8_t sync_index, uint16_t pdo_index)

    68

{

    69

    ec_ioctl_config_pdo_t data;

    70

    71

    data.config_index = sc->index;

    72

    data.sync_index = sync_index;

    73

    data.index = pdo_index;

    74

    75

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

    76

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

    77

                strerror(errno));

    78

        return -1;  // FIXME

    79

    }

    80

    81

    return 0;

    82

}

    83

    84

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

    85

    86

void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc,

    87

        uint8_t sync_index)

    88

{

    89

    ec_ioctl_config_pdo_t data;

    90

    91

    data.config_index = sc->index;

    92

    data.sync_index = sync_index;

    93

    94

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

    95

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

    96

                strerror(errno));

    97

    }

    98

}

    99

   100

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

   101

   102

int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc,

   103

        uint16_t pdo_index, uint16_t entry_index, uint8_t entry_subindex,

   104

        uint8_t entry_bit_length)

   105

{

   106

    ec_ioctl_add_pdo_entry_t data;

   107

   108

    data.config_index = sc->index;

   109

    data.pdo_index = pdo_index;

   110

    data.entry_index = entry_index;

   111

    data.entry_subindex = entry_subindex;

   112

    data.entry_bit_length = entry_bit_length;

   113

   114

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

   115

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

   116

                strerror(errno));

   117

        return -1;  // FIXME

   118

    }

   119

   120

    return 0;

   121

}

   122

   123

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

   124

   125

void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc,

   126

        uint16_t pdo_index)

   127

{

   128

    ec_ioctl_config_pdo_t data;

   129

   130

    data.config_index = sc->index;

   131

    data.index = pdo_index;

   132

   133

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

   134

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

   135

                strerror(errno));

   136

    }

   137

}

   138

   139

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

   140

   141

int ecrt_slave_config_pdos(ec_slave_config_t *sc,

   142

        unsigned int n_syncs, const ec_sync_info_t syncs[])

   143

{

   144

    int ret;

   145

    unsigned int i, j, k;

   146

    const ec_sync_info_t *sync_info;

   147

    const ec_pdo_info_t *pdo_info;

   148

    const ec_pdo_entry_info_t *entry_info;

   149

   150

    if (!syncs)

   151

        return 0;

   152

   153

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

   154

        sync_info = &syncs[i];

   155

   156

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

   157

            break;

   158

   159

        if (sync_info->index >= EC_MAX_SYNC_MANAGERS) {

   160

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

   161

                    sync_info->index);

   162

            return -ENOENT;

   163

        }

   164

   165

        ret = ecrt_slave_config_sync_manager(

   166

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

   167

        if (ret)

   168

            return ret;

   169

   170

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

   171

            ecrt_slave_config_pdo_assign_clear(sc, sync_info->index);

   172

   173

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

   174

                pdo_info = &sync_info->pdos[j];

   175

   176

                ret = ecrt_slave_config_pdo_assign_add(

   177

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

   178

                if (ret)

   179

                    return ret;

   180

   181

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

   182

                    ecrt_slave_config_pdo_mapping_clear(sc, pdo_info->index);

   183

   184

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

   185

                        entry_info = &pdo_info->entries[k];

   186

   187

                        ret = ecrt_slave_config_pdo_mapping_add(sc,

   188

                                pdo_info->index, entry_info->index,

   189

                                entry_info->subindex,

   190

                                entry_info->bit_length);

   191

                        if (ret)

   192

                            return ret;

   193

                    }

   194

                }

   195

            }

   196

        }

   197

    }

   198

   199

    return 0;

   200

}

   201

   202

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

   203

   204

int ecrt_slave_config_reg_pdo_entry(

   205

        ec_slave_config_t *sc,

   206

        uint16_t index,

   207

        uint8_t subindex,

   208

        ec_domain_t *domain,

   209

        unsigned int *bit_position

   210

        )

   211

{

   212

    ec_ioctl_reg_pdo_entry_t data;

   213

    int ret;

   214

   215

    data.config_index = sc->index;

   216

    data.entry_index = index;

   217

    data.entry_subindex = subindex;

   218

    data.domain_index = domain->index;

   219

   220

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

   221

    if (ret == -1) {

   222

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

   223

                strerror(errno));

   224

        return -2; // FIXME

   225

    }

   226

   227

    if (bit_position) {

   228

        *bit_position = data.bit_position;

   229

    } else {

   230

        if (data.bit_position) {

   231

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

   232

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

   233

                    sc->alias, sc->position);

   234

            return -3; // FIXME

   235

        }

   236

    }

   237

   238

    return ret;

   239

}

   240

   241

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

   242

   243

int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index,

   244

        uint8_t subindex, const uint8_t *sdo_data, size_t size)

   245

{

   246

    ec_ioctl_sc_sdo_t data;

   247

   248

    data.config_index = sc->index;

   249

    data.index = index;

   250

    data.subindex = subindex;

   251

    data.data = sdo_data;

   252

    data.size = size;

   253

   254

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

   255

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

   256

        return -1; // FIXME

   257

    }

   258

   259

    return 0;

   260

}

   261

   262

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

   263

   264

int ecrt_slave_config_sdo8(ec_slave_config_t *sc, uint16_t index,

   265

        uint8_t subindex, uint8_t value)

   266

{

   267

    uint8_t data[1];

   268

   269

    EC_WRITE_U8(data, value);

   270

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

   271

}

   272

   273

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

   274

   275

int ecrt_slave_config_sdo16(ec_slave_config_t *sc, uint16_t index,

   276

        uint8_t subindex, uint16_t value)

   277

{

   278

    uint8_t data[2];

   279

   280

    EC_WRITE_U16(data, value);

   281

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

   282

}

   283

   284

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

   285

   286

int ecrt_slave_config_sdo32(ec_slave_config_t *sc, uint16_t index,

   287

        uint8_t subindex, uint32_t value)

   288

{

   289

    uint8_t data[4];

   290

   291

    EC_WRITE_U32(data, value);

   292

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

   293

}

   294

   295

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

   296

   297

ec_sdo_request_t *ecrt_slave_config_create_sdo_request(ec_slave_config_t *sc,

   298

        uint16_t index, uint8_t subindex, size_t size)

   299

{

   300

    return 0; // TODO

   301

}

   302

   303

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

   304

   305

ec_voe_handler_t *ecrt_slave_config_create_voe_handler(ec_slave_config_t *sc,

   306

        size_t size)

   307

{

   308

    ec_ioctl_voe_t data;

   309

    ec_voe_handler_t *voe;

   310

    unsigned int index;

   311

   312

    voe = malloc(sizeof(ec_voe_handler_t));

   313

    if (!voe) {

   314

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

   315

        return 0;

   316

    }

   317

   318

    if (size) {

   319

        voe->data = malloc(size);

   320

        if (!voe->data) {

   321

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

   322

                    " memory.\n", size);

   323

            free(voe);

   324

            return 0;

   325

        }

   326

    } else {

   327

        voe->data = NULL;

   328

    }

   329

   330

    data.config_index = sc->index;

   331

    data.size = size;

   332

   333

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

   334

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

   335

                strerror(errno));

   336

        if (voe->data)

   337

            free(voe->data);

   338

        free(voe);

   339

        return NULL; 

   340

    }

   341

   342

    voe->config = sc;

   343

    voe->index = data.voe_index;

   344

    voe->data_size = size;

   345

    voe->mem_size = size;

   346

    return voe;

   347

}

   348

   349

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

   350

   351

void ecrt_slave_config_state(const ec_slave_config_t *sc,

   352

        ec_slave_config_state_t *state)

   353

{

   354

    ec_ioctl_sc_state_t data;

   355

   356

    data.config_index = sc->index;

   357

    data.state = state;

   358

   359

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

   360

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

   361

                strerror(errno));

   362

    }

   363

}

   364

   365

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