1

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

     2

 *  

     3

 * $Id$

     4

 * 

     5

 * Copyright (C) 2006-2008  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 of

    12

 * 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

 * The right to use EtherCAT Technology is granted and comes free of charge

    24

 * under condition of compatibility of product made by Licensee. People

    25

 * intending to distribute/sell products based on the code, have to sign an

    26

 * agreement to guarantee that products using software based on IgH EtherCAT

    27

 * master stay compatible with the actual EtherCAT specification (which are

    28

 * released themselves as an open standard) as the (only) precondition to have

    29

 * the right to use EtherCAT Technology, IP and trade marks.

    30

 *

    31

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

    32

    33

#include <stdlib.h>

    34

#include <sys/ioctl.h>

    35

#include <stdio.h>

    36

#include <errno.h>

    37

#include <string.h>

    38

    39

#include "slave_config.h"

    40

#include "domain.h"

    41

#include "voe_handler.h"

    42

#include "master.h"

    43

#include "master/ioctl.h"

    44

    45

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

    46

    47

int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index,

    48

        ec_direction_t dir)

    49

{

    50

    ec_ioctl_config_t data;

    51

    unsigned int i;

    52

    53

    if (sync_index >= EC_MAX_SYNC_MANAGERS)

    54

        return -ENOENT;

    55

    56

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

    57

    data.config_index = sc->index;

    58

    data.syncs[sync_index].dir = dir;

    59

    60

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

    61

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

    62

                strerror(errno));

    63

        return -1; // FIXME

    64

    }

    65

    66

    return 0;

    67

}

    68

    69

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

    70

    71

int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc,

    72

        uint8_t sync_index, uint16_t pdo_index)

    73

{

    74

    ec_ioctl_config_pdo_t data;

    75

    76

    data.config_index = sc->index;

    77

    data.sync_index = sync_index;

    78

    data.index = pdo_index;

    79

    80

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

    81

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

    82

                strerror(errno));

    83

        return -1;  // FIXME

    84

    }

    85

    86

    return 0;

    87

}

    88

    89

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

    90

    91

void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc,

    92

        uint8_t sync_index)

    93

{

    94

    ec_ioctl_config_pdo_t data;

    95

    96

    data.config_index = sc->index;

    97

    data.sync_index = sync_index;

    98

    99

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

   100

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

   101

                strerror(errno));

   102

    }

   103

}

   104

   105

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

   106

   107

int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc,

   108

        uint16_t pdo_index, uint16_t entry_index, uint8_t entry_subindex,

   109

        uint8_t entry_bit_length)

   110

{

   111

    ec_ioctl_add_pdo_entry_t data;

   112

   113

    data.config_index = sc->index;

   114

    data.pdo_index = pdo_index;

   115

    data.entry_index = entry_index;

   116

    data.entry_subindex = entry_subindex;

   117

    data.entry_bit_length = entry_bit_length;

   118

   119

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

   120

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

   121

                strerror(errno));

   122

        return -1;  // FIXME

   123

    }

   124

   125

    return 0;

   126

}

   127

   128

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

   129

   130

void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc,

   131

        uint16_t pdo_index)

   132

{

   133

    ec_ioctl_config_pdo_t data;

   134

   135

    data.config_index = sc->index;

   136

    data.index = pdo_index;

   137

   138

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

   139

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

   140

                strerror(errno));

   141

    }

   142

}

   143

   144

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

   145

   146

int ecrt_slave_config_pdos(ec_slave_config_t *sc,

   147

        unsigned int n_syncs, const ec_sync_info_t syncs[])

   148

{

   149

    int ret;

   150

    unsigned int i, j, k;

   151

    const ec_sync_info_t *sync_info;

   152

    const ec_pdo_info_t *pdo_info;

   153

    const ec_pdo_entry_info_t *entry_info;

   154

   155

    if (!syncs)

   156

        return 0;

   157

   158

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

   159

        sync_info = &syncs[i];

   160

   161

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

   162

            break;

   163

   164

        if (sync_info->index >= EC_MAX_SYNC_MANAGERS) {

   165

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

   166

                    sync_info->index);

   167

            return -ENOENT;

   168

        }

   169

   170

        ret = ecrt_slave_config_sync_manager(

   171

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

   172

        if (ret)

   173

            return ret;

   174

   175

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

   176

            ecrt_slave_config_pdo_assign_clear(sc, sync_info->index);

   177

   178

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

   179

                pdo_info = &sync_info->pdos[j];

   180

   181

                ret = ecrt_slave_config_pdo_assign_add(

   182

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

   183

                if (ret)

   184

                    return ret;

   185

   186

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

   187

                    ecrt_slave_config_pdo_mapping_clear(sc, pdo_info->index);

   188

   189

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

   190

                        entry_info = &pdo_info->entries[k];

   191

   192

                        ret = ecrt_slave_config_pdo_mapping_add(sc,

   193

                                pdo_info->index, entry_info->index,

   194

                                entry_info->subindex,

   195

                                entry_info->bit_length);

   196

                        if (ret)

   197

                            return ret;

   198

                    }

   199

                }

   200

            }

   201

        }

   202

    }

   203

   204

    return 0;

   205

}

   206

   207

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

   208

   209

int ecrt_slave_config_reg_pdo_entry(

   210

        ec_slave_config_t *sc,

   211

        uint16_t index,

   212

        uint8_t subindex,

   213

        ec_domain_t *domain,

   214

        unsigned int *bit_position

   215

        )

   216

{

   217

    ec_ioctl_reg_pdo_entry_t data;

   218

    int ret;

   219

   220

    data.config_index = sc->index;

   221

    data.entry_index = index;

   222

    data.entry_subindex = subindex;

   223

    data.domain_index = domain->index;

   224

   225

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

   226

    if (ret == -1) {

   227

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

   228

                strerror(errno));

   229

        return -2; // FIXME

   230

    }

   231

   232

    if (bit_position) {

   233

        *bit_position = data.bit_position;

   234

    } else {

   235

        if (data.bit_position) {

   236

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

   237

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

   238

                    sc->alias, sc->position);

   239

            return -3; // FIXME

   240

        }

   241

    }

   242

   243

    return ret;

   244

}

   245

   246

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

   247

   248

int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index,

   249

        uint8_t subindex, const uint8_t *sdo_data, size_t size)

   250

{

   251

    ec_ioctl_sc_sdo_t data;

   252

   253

    data.config_index = sc->index;

   254

    data.index = index;

   255

    data.subindex = subindex;

   256

    data.data = sdo_data;

   257

    data.size = size;

   258

   259

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

   260

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

   261

        return -1; // FIXME

   262

    }

   263

   264

    return 0;

   265

}

   266

   267

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

   268

   269

int ecrt_slave_config_sdo8(ec_slave_config_t *sc, uint16_t index,

   270

        uint8_t subindex, uint8_t value)

   271

{

   272

    uint8_t data[1];

   273

   274

    EC_WRITE_U8(data, value);

   275

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

   276

}

   277

   278

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

   279

   280

int ecrt_slave_config_sdo16(ec_slave_config_t *sc, uint16_t index,

   281

        uint8_t subindex, uint16_t value)

   282

{

   283

    uint8_t data[2];

   284

   285

    EC_WRITE_U16(data, value);

   286

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

   287

}

   288

   289

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

   290

   291

int ecrt_slave_config_sdo32(ec_slave_config_t *sc, uint16_t index,

   292

        uint8_t subindex, uint32_t value)

   293

{

   294

    uint8_t data[4];

   295

   296

    EC_WRITE_U32(data, value);

   297

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

   298

}

   299

   300

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

   301

   302

ec_sdo_request_t *ecrt_slave_config_create_sdo_request(ec_slave_config_t *sc,

   303

        uint16_t index, uint8_t subindex, size_t size)

   304

{

   305

    return 0; // TODO

   306

}

   307

   308

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

   309

   310

ec_voe_handler_t *ecrt_slave_config_create_voe_handler(ec_slave_config_t *sc,

   311

        size_t size)

   312

{

   313

    ec_ioctl_voe_t data;

   314

    ec_voe_handler_t *voe;

   315

    unsigned int index;

   316

   317

    voe = malloc(sizeof(ec_voe_handler_t));

   318

    if (!voe) {

   319

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

   320

        return 0;

   321

    }

   322

   323

    if (size) {

   324

        voe->data = malloc(size);

   325

        if (!voe->data) {

   326

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

   327

                    " memory.\n", size);

   328

            free(voe);

   329

            return 0;

   330

        }

   331

    } else {

   332

        voe->data = NULL;

   333

    }

   334

   335

    data.config_index = sc->index;

   336

    data.size = size;

   337

   338

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

   339

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

   340

                strerror(errno));

   341

        if (voe->data)

   342

            free(voe->data);

   343

        free(voe);

   344

        return NULL; 

   345

    }

   346

   347

    voe->config = sc;

   348

    voe->index = data.voe_index;

   349

    voe->data_size = size;

   350

    voe->mem_size = size;

   351

    return voe;

   352

}

   353

   354

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

   355

   356

void ecrt_slave_config_state(const ec_slave_config_t *sc,

   357

        ec_slave_config_state_t *state)

   358

{

   359

    ec_ioctl_sc_state_t data;

   360

   361

    data.config_index = sc->index;

   362

    data.state = state;

   363

   364

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

   365

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

   366

                strerror(errno));

   367

    }

   368

}

   369

   370

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