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.

     8

 *

     9

 *  The IgH EtherCAT Master is free software; you can redistribute it and/or

    10

 *  modify it under the terms of the GNU General Public License version 2, as

    11

 *  published by the Free Software Foundation.

    12

 *

    13

 *  The IgH EtherCAT Master is distributed in the hope that it will be useful,

    14

 *  but WITHOUT ANY WARRANTY; without even the implied warranty of

    15

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General

    16

 *  Public License for more details.

    17

 *

    18

 *  You should have received a copy of the GNU General Public License along

    19

 *  with the IgH EtherCAT Master; if not, write to the Free Software

    20

 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    21

 *

    22

 *  ---

    23

 *

    24

 *  The license mentioned above concerns the source code only. Using the

    25

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

    26

 *  industrial property and similar rights of Beckhoff Automation GmbH.

    27

 *

    28

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

    29

    30

/**

    31

   \file

    32

   EtherCAT slave configuration methods.

    33

*/

    34

    35

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

    36

    37

#include <linux/slab.h>

    38

    39

#include "globals.h"

    40

#include "master.h"

    41

#include "voe_handler.h"

    42

    43

#include "slave_config.h"

    44

    45

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

    46

    47

/** Slave configuration constructor.

    48

 *

    49

 * See ecrt_master_slave_config() for the usage of the \a alias and \a

    50

 * position parameters.

    51

 */

    52

void ec_slave_config_init(

    53

        ec_slave_config_t *sc, /**< Slave configuration. */

    54

        ec_master_t *master, /**< EtherCAT master. */

    55

        uint16_t alias, /**< Slave alias. */

    56

        uint16_t position, /**< Slave position. */

    57

        uint32_t vendor_id, /**< Expected vendor ID. */

    58

        uint32_t product_code /**< Expected product code. */

    59

        )

    60

{

    61

    unsigned int i;

    62

    63

    sc->master = master;

    64

    sc->alias = alias;

    65

    sc->position = position;

    66

    sc->vendor_id = vendor_id;

    67

    sc->product_code = product_code;

    68

    sc->slave = NULL;

    69

    70

    for (i = 0; i < EC_MAX_SYNC_MANAGERS; i++)

    71

        ec_sync_config_init(&sc->sync_configs[i]);

    72

    73

    INIT_LIST_HEAD(&sc->sdo_configs);

    74

    INIT_LIST_HEAD(&sc->sdo_requests);

    75

    INIT_LIST_HEAD(&sc->voe_handlers);

    76

    77

    sc->used_fmmus = 0;

    78

}

    79

    80

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

    81

    82

/** Slave configuration destructor.

    83

 *

    84

 * Clears and frees a slave configuration object.

    85

 */

    86

void ec_slave_config_clear(

    87

        ec_slave_config_t *sc /**< Slave configuration. */

    88

        )

    89

{

    90

    unsigned int i;

    91

    ec_sdo_request_t *req, *next_req;

    92

    ec_voe_handler_t *voe, *next_voe;

    93

    94

    ec_slave_config_detach(sc);

    95

    96

    // Free sync managers

    97

    for (i = 0; i < EC_MAX_SYNC_MANAGERS; i++)

    98

        ec_sync_config_clear(&sc->sync_configs[i]);

    99

   100

    // free all SDO configurations

   101

    list_for_each_entry_safe(req, next_req, &sc->sdo_configs, list) {

   102

        list_del(&req->list);

   103

        ec_sdo_request_clear(req);

   104

        kfree(req);

   105

    }

   106

   107

    // free all SDO requests

   108

    list_for_each_entry_safe(req, next_req, &sc->sdo_requests, list) {

   109

        list_del(&req->list);

   110

        ec_sdo_request_clear(req);

   111

        kfree(req);

   112

    }

   113

   114

    // free all VoE handlers

   115

    list_for_each_entry_safe(voe, next_voe, &sc->voe_handlers, list) {

   116

        list_del(&voe->list);

   117

        ec_voe_handler_clear(voe);

   118

        kfree(voe);

   119

    }

   120

}

   121

   122

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

   123

   124

/** Prepares an FMMU configuration.

   125

 *

   126

 * Configuration data for the FMMU is saved in the slave config structure and

   127

 * is written to the slave during the configuration. The FMMU configuration

   128

 * is done in a way, that the complete data range of the corresponding sync

   129

 * manager is covered. Seperate FMMUs are configured for each domain. If the

   130

 * FMMU configuration is already prepared, the function does nothing and

   131

 * returns with success.

   132

 *

   133

 * \retval >=0 Success, logical offset byte address.

   134

 * \retval  <0 Error code.

   135

 */

   136

int ec_slave_config_prepare_fmmu(

   137

        ec_slave_config_t *sc, /**< Slave configuration. */

   138

        ec_domain_t *domain, /**< Domain. */

   139

        uint8_t sync_index, /**< Sync manager index. */

   140

        ec_direction_t dir /**< PDO direction. */

   141

        )

   142

{

   143

    unsigned int i;

   144

    ec_fmmu_config_t *fmmu;

   145

   146

    // FMMU configuration already prepared?

   147

    for (i = 0; i < sc->used_fmmus; i++) {

   148

        fmmu = &sc->fmmu_configs[i];

   149

        if (fmmu->domain == domain && fmmu->sync_index == sync_index)

   150

            return fmmu->logical_start_address;

   151

    }

   152

   153

    if (sc->used_fmmus == EC_MAX_FMMUS) {

   154

        EC_ERR("FMMU limit reached for slave configuration %u:%u!\n",

   155

                sc->alias, sc->position);

   156

        return -EOVERFLOW;

   157

    }

   158

   159

    fmmu = &sc->fmmu_configs[sc->used_fmmus++];

   160

   161

    down(&sc->master->master_sem);

   162

    ec_fmmu_config_init(fmmu, sc, domain, sync_index, dir);

   163

    up(&sc->master->master_sem);

   164

   165

    return fmmu->logical_start_address;

   166

}

   167

   168

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

   169

   170

/** Attaches the configuration to the addressed slave object.

   171

 *

   172

 * \retval  0 Success.

   173

 * \retval <0 Error code.

   174

 */

   175

int ec_slave_config_attach(

   176

        ec_slave_config_t *sc /**< Slave configuration. */

   177

        )

   178

{

   179

	ec_slave_t *slave;

   180

   181

	if (sc->slave)

   182

		return 0; // already attached

   183

   184

    if (!(slave = ec_master_find_slave(

   185

                    sc->master, sc->alias, sc->position))) {

   186

        if (sc->master->debug_level)

   187

            EC_DBG("Failed to find slave for configuration %u:%u.\n",

   188

                    sc->alias, sc->position);

   189

        return -ENOENT;

   190

    }

   191

   192

	if (slave->config) {

   193

        if (sc->master->debug_level)

   194

            EC_DBG("Failed to attach slave configuration %u:%u. Slave %u"

   195

                    " already has a configuration!\n", sc->alias,

   196

                    sc->position, slave->ring_position);

   197

        return -EEXIST;

   198

    }

   199

   200

    if (slave->sii.vendor_id != sc->vendor_id

   201

            || slave->sii.product_code != sc->product_code) {

   202

        if (sc->master->debug_level)

   203

            EC_DBG("Slave %u has an invalid type (0x%08X/0x%08X) for"

   204

                    " configuration %u:%u (0x%08X/0x%08X).\n",

   205

                    slave->ring_position, slave->sii.vendor_id,

   206

                    slave->sii.product_code, sc->alias, sc->position,

   207

                    sc->vendor_id, sc->product_code);

   208

        return -EINVAL;

   209

	}

   210

   211

	// attach slave

   212

	slave->config = sc;

   213

	sc->slave = slave;

   214

   215

    ec_slave_request_state(slave, EC_SLAVE_STATE_OP);

   216

   217

    if (sc->master->debug_level)

   218

        EC_DBG("Attached slave %u to config %u:%u.\n",

   219

                slave->ring_position, sc->alias, sc->position);

   220

   221

	return 0;

   222

}

   223

   224

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

   225

   226

/** Detaches the configuration from a slave object.

   227

 */

   228

void ec_slave_config_detach(

   229

        ec_slave_config_t *sc /**< Slave configuration. */

   230

        )

   231

{

   232

    if (sc->slave) {

   233

        sc->slave->config = NULL;

   234

        sc->slave = NULL;

   235

    }

   236

}

   237

   238

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

   239

   240

/** Loads the default PDO assignment from the slave object.

   241

 */

   242

void ec_slave_config_load_default_sync_config(ec_slave_config_t *sc)

   243

{

   244

    uint8_t sync_index;

   245

    ec_sync_config_t *sync_config;

   246

    const ec_sync_t *sync;

   247

   248

    if (!sc->slave)

   249

        return;

   250

   251

    for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) {

   252

        sync_config = &sc->sync_configs[sync_index];

   253

        if ((sync = ec_slave_get_sync(sc->slave, sync_index))) {

   254

            sync_config->dir = ec_sync_default_direction(sync);

   255

            if (sync_config->dir == EC_DIR_INVALID)

   256

                EC_WARN("SM%u of slave %u has an invalid direction field!\n",

   257

                        sync_index, sc->slave->ring_position);

   258

            ec_pdo_list_copy(&sync_config->pdos, &sync->pdos);

   259

        }

   260

    }

   261

}

   262

   263

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

   264

   265

/** Loads the default mapping for a PDO from the slave object.

   266

 */

   267

void ec_slave_config_load_default_mapping(

   268

        const ec_slave_config_t *sc,

   269

        ec_pdo_t *pdo

   270

        )

   271

{

   272

    unsigned int i;

   273

    const ec_sync_t *sync;

   274

    const ec_pdo_t *default_pdo;

   275

   276

    if (!sc->slave)

   277

        return;

   278

   279

    if (sc->master->debug_level)

   280

        EC_DBG("Loading default mapping for PDO 0x%04X in config %u:%u.\n",

   281

                pdo->index, sc->alias, sc->position);

   282

   283

    // find PDO in any sync manager (it could be reassigned later)

   284

    for (i = 0; i < sc->slave->sii.sync_count; i++) {

   285

        sync = &sc->slave->sii.syncs[i];

   286

   287

        list_for_each_entry(default_pdo, &sync->pdos.list, list) {

   288

            if (default_pdo->index != pdo->index)

   289

                continue;

   290

   291

            if (default_pdo->name) {

   292

                if (sc->master->debug_level)

   293

                    EC_DBG("Found PDO name \"%s\".\n", default_pdo->name);

   294

   295

                // take PDO name from assigned one

   296

                ec_pdo_set_name(pdo, default_pdo->name);

   297

            }

   298

   299

            // copy entries (= default PDO mapping)

   300

            if (ec_pdo_copy_entries(pdo, default_pdo))

   301

                return;

   302

   303

            if (sc->master->debug_level) {

   304

                const ec_pdo_entry_t *entry;

   305

                list_for_each_entry(entry, &pdo->entries, list) {

   306

                    EC_DBG("Entry 0x%04X:%02X.\n",

   307

                            entry->index, entry->subindex);

   308

                }

   309

            }

   310

   311

            return;

   312

        }

   313

    }

   314

   315

    if (sc->master->debug_level)

   316

        EC_DBG("No default mapping found.\n");

   317

}

   318

   319

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

   320

   321

/** Get the number of SDO configurations.

   322

 *

   323

 * \return Number of SDO configurations.

   324

 */

   325

unsigned int ec_slave_config_sdo_count(

   326

        const ec_slave_config_t *sc /**< Slave configuration. */

   327

        )

   328

{

   329

	const ec_sdo_request_t *req;

   330

	unsigned int count = 0;

   331

   332

	list_for_each_entry(req, &sc->sdo_configs, list) {

   333

		count++;

   334

	}

   335

   336

	return count;

   337

}

   338

   339

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

   340

   341

/** Finds an SDO configuration via its position in the list.

   342

 *

   343

 * Const version.

   344

 */

   345

const ec_sdo_request_t *ec_slave_config_get_sdo_by_pos_const(

   346

        const ec_slave_config_t *sc, /**< Slave configuration. */

   347

        unsigned int pos /**< Position in the list. */

   348

        )

   349

{

   350

    const ec_sdo_request_t *req;

   351

   352

    list_for_each_entry(req, &sc->sdo_configs, list) {

   353

        if (pos--)

   354

            continue;

   355

        return req;

   356

    }

   357

   358

    return NULL;

   359

}

   360

   361

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

   362

   363

/** Finds a VoE handler via its position in the list.

   364

 */

   365

ec_sdo_request_t *ec_slave_config_find_sdo_request(

   366

        ec_slave_config_t *sc, /**< Slave configuration. */

   367

        unsigned int pos /**< Position in the list. */

   368

        )

   369

{

   370

    ec_sdo_request_t *req;

   371

   372

    list_for_each_entry(req, &sc->sdo_requests, list) {

   373

        if (pos--)

   374

            continue;

   375

        return req;

   376

    }

   377

   378

    return NULL;

   379

}

   380

   381

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

   382

   383

/** Finds a VoE handler via its position in the list.

   384

 */

   385

ec_voe_handler_t *ec_slave_config_find_voe_handler(

   386

        ec_slave_config_t *sc, /**< Slave configuration. */

   387

        unsigned int pos /**< Position in the list. */

   388

        )

   389

{

   390

    ec_voe_handler_t *voe;

   391

   392

    list_for_each_entry(voe, &sc->voe_handlers, list) {

   393

        if (pos--)

   394

            continue;

   395

        return voe;

   396

    }

   397

   398

    return NULL;

   399

}

   400

   401

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

   402

 *  Realtime interface

   403

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

   404

   405

int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index,

   406

        ec_direction_t dir)

   407

{

   408

    ec_sync_config_t *sync_config;

   409

   410

    if (sc->master->debug_level)

   411

        EC_DBG("ecrt_slave_config_sync_manager(sc = 0x%x, sync_index = %u, "

   412

                "dir = %u)\n", (u32) sc, sync_index, dir);

   413

   414

    if (sync_index >= EC_MAX_SYNC_MANAGERS) {

   415

        EC_ERR("Invalid sync manager index %u!\n", sync_index);

   416

        return -ENOENT;

   417

    }

   418

   419

    if (dir != EC_DIR_OUTPUT && dir != EC_DIR_INPUT) {

   420

        EC_ERR("Invalid direction %u!\n", (u32) dir);

   421

        return -EINVAL;

   422

    }

   423

   424

    sync_config = &sc->sync_configs[sync_index];

   425

    sync_config->dir = dir;

   426

    return 0;

   427

}

   428

   429

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

   430

   431

int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc,

   432

        uint8_t sync_index, uint16_t pdo_index)

   433

{

   434

    ec_pdo_t *pdo;

   435

   436

    if (sc->master->debug_level)

   437

        EC_DBG("ecrt_slave_config_pdo_assign_add(sc = 0x%x, sync_index = %u, "

   438

                "pdo_index = 0x%04X)\n", (u32) sc, sync_index, pdo_index);

   439

   440

    if (sync_index >= EC_MAX_SYNC_MANAGERS) {

   441

        EC_ERR("Invalid sync manager index %u!\n", sync_index);

   442

        return -EINVAL;

   443

    }

   444

   445

    down(&sc->master->master_sem);

   446

   447

    pdo = ec_pdo_list_add_pdo(&sc->sync_configs[sync_index].pdos, pdo_index);

   448

    if (IS_ERR(pdo)) {

   449

        up(&sc->master->master_sem);

   450

        return PTR_ERR(pdo);

   451

    }

   452

    pdo->sync_index = sync_index;

   453

   454

    ec_slave_config_load_default_mapping(sc, pdo);

   455

   456

    up(&sc->master->master_sem);

   457

    return 0;

   458

}

   459

   460

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

   461

   462

void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc,

   463

        uint8_t sync_index)

   464

{

   465

    if (sc->master->debug_level)

   466

        EC_DBG("ecrt_slave_config_pdo_assign_clear(sc = 0x%x, "

   467

                "sync_index = %u)\n", (u32) sc, sync_index);

   468

   469

    if (sync_index >= EC_MAX_SYNC_MANAGERS) {

   470

        EC_ERR("Invalid sync manager index %u!\n", sync_index);

   471

        return;

   472

    }

   473

   474

    down(&sc->master->master_sem);

   475

    ec_pdo_list_clear_pdos(&sc->sync_configs[sync_index].pdos);

   476

    up(&sc->master->master_sem);

   477

}

   478

   479

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

   480

   481

int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc,

   482

        uint16_t pdo_index, uint16_t entry_index, uint8_t entry_subindex,

   483

        uint8_t entry_bit_length)

   484

{

   485

    uint8_t sync_index;

   486

    ec_pdo_t *pdo = NULL;

   487

    ec_pdo_entry_t *entry;

   488

    int retval = 0;

   489

   490

    if (sc->master->debug_level)

   491

        EC_DBG("ecrt_slave_config_pdo_mapping_add(sc = 0x%x, "

   492

                "pdo_index = 0x%04X, entry_index = 0x%04X, "

   493

                "entry_subindex = 0x%02X, entry_bit_length = %u)\n",

   494

                (u32) sc, pdo_index, entry_index, entry_subindex,

   495

                entry_bit_length);

   496