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 domain methods.

    33

*/

    34

    35

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

    36

    37

#include <linux/module.h>

    38

    39

#include "globals.h"

    40

#include "master.h"

    41

#include "slave_config.h"

    42

    43

#include "domain.h"

    44

#include "datagram_pair.h"

    45

    46

/** Extra debug output for redundancy functions.

    47

 */

    48

#define DEBUG_REDUNDANCY 0

    49

    50

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

    51

    52

void ec_domain_clear_data(ec_domain_t *);

    53

    54

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

    55

    56

/** Domain constructor.

    57

 */

    58

void ec_domain_init(

    59

        ec_domain_t *domain, /**< EtherCAT domain. */

    60

        ec_master_t *master, /**< Parent master. */

    61

        unsigned int index /**< Index. */

    62

        )

    63

{

    64

    unsigned int dev_idx;

    65

    66

    domain->master = master;

    67

    domain->index = index;

    68

    INIT_LIST_HEAD(&domain->fmmu_configs);

    69

    domain->data_size = 0;

    70

    domain->data = NULL;

    71

    domain->data_origin = EC_ORIG_INTERNAL;

    72

    domain->logical_base_address = 0x00000000;

    73

    INIT_LIST_HEAD(&domain->datagram_pairs);

    74

    for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master);

    75

            dev_idx++) {

    76

        domain->working_counter[dev_idx] = 0x0000;

    77

    }

    78

    domain->expected_working_counter = 0x0000;

    79

    domain->working_counter_changes = 0;

    80

    domain->redundancy_active = 0;

    81

    domain->notify_jiffies = 0;

    82

    83

    /* Used by ec_domain_add_fmmu_config */

    84

    memset(domain->offset_used, 0, sizeof(domain->offset_used));

    85

    domain->sc_in_work = 0;

    86

}

    87

    88

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

    89

    90

/** Domain destructor.

    91

 */

    92

void ec_domain_clear(ec_domain_t *domain /**< EtherCAT domain */)

    93

{

    94

    ec_datagram_pair_t *datagram_pair, *next_pair;

    95

    96

    // dequeue and free datagrams

    97

    list_for_each_entry_safe(datagram_pair, next_pair,

    98

            &domain->datagram_pairs, list) {

    99

        ec_datagram_pair_clear(datagram_pair);

   100

        kfree(datagram_pair);

   101

    }

   102

   103

    ec_domain_clear_data(domain);

   104

}

   105

   106

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

   107

   108

/** Frees internally allocated memory.

   109

 */

   110

void ec_domain_clear_data(

   111

        ec_domain_t *domain /**< EtherCAT domain. */

   112

        )

   113

{

   114

    if (domain->data_origin == EC_ORIG_INTERNAL && domain->data) {

   115

        kfree(domain->data);

   116

    }

   117

   118

    domain->data = NULL;

   119

    domain->data_origin = EC_ORIG_INTERNAL;

   120

}

   121

   122

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

   123

   124

/** Adds an FMMU configuration to the domain.

   125

 */

   126

void ec_domain_add_fmmu_config(

   127

        ec_domain_t *domain, /**< EtherCAT domain. */

   128

        ec_fmmu_config_t *fmmu /**< FMMU configuration. */

   129

        )

   130

{

   131

    const ec_slave_config_t *sc;

   132

    uint32_t logical_domain_offset;

   133

    unsigned fmmu_data_size;

   134

   135

    fmmu->domain = domain;

   136

    sc = fmmu->sc;

   137

   138

    fmmu_data_size = ec_pdo_list_total_size(

   139

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

   140

   141

    if (sc->allow_overlapping_pdos && (sc == domain->sc_in_work)) {

   142

        // If we permit overlapped PDOs, and we already have an allocated FMMU

   143

        // for this slave, allocate the subsequent FMMU offsets by direction

   144

        logical_domain_offset = domain->offset_used[fmmu->dir];

   145

    } else {

   146

        // otherwise, allocate to the furthest extent of any allocated

   147

        // FMMU on this domain.

   148

        logical_domain_offset = max(domain->offset_used[EC_DIR_INPUT],

   149

            domain->offset_used[EC_DIR_OUTPUT]);

   150

        // rebase the free offsets to the current position

   151

        domain->offset_used[EC_DIR_INPUT] = logical_domain_offset;

   152

        domain->offset_used[EC_DIR_OUTPUT] = logical_domain_offset;

   153

    }

   154

    domain->sc_in_work = sc;

   155

   156

    // consume the offset space for this FMMU's direction

   157

    domain->offset_used[fmmu->dir] += fmmu_data_size;

   158

   159

    ec_fmmu_set_domain_offset_size(fmmu, logical_domain_offset, fmmu_data_size);

   160

   161

    list_add_tail(&fmmu->list, &domain->fmmu_configs);

   162

   163

    // Determine domain size from furthest extent of FMMU data

   164

    domain->data_size = max(domain->offset_used[EC_DIR_INPUT],

   165

            domain->offset_used[EC_DIR_OUTPUT]);

   166

   167

    EC_MASTER_DBG(domain->master, 1, "Domain %u:"

   168

            " Added %u bytes at %u.\n",

   169

            domain->index, fmmu->data_size, logical_domain_offset);

   170

}

   171

   172

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

   173

   174

/** Allocates a domain datagram pair and appends it to the list.

   175

 *

   176

 * The datagrams' types and expected working counters are determined by the

   177

 * number of input and output fmmus that share the datagrams.

   178

 *

   179

 * \retval  0 Success.

   180

 * \retval <0 Error code.

   181

 */

   182

int ec_domain_add_datagram_pair(

   183

        ec_domain_t *domain, /**< EtherCAT domain. */

   184

        uint32_t logical_offset, /**< Logical offset. */

   185

        size_t data_size, /**< Size of the data. */

   186

        uint8_t *data, /**< Process data. */

   187

        const unsigned int used[] /**< Slave config counter for in/out. */

   188

        )

   189

{

   190

    ec_datagram_pair_t *datagram_pair;

   191

    int ret;

   192

   193

    if (!(datagram_pair = kmalloc(sizeof(ec_datagram_pair_t), GFP_KERNEL))) {

   194

        EC_MASTER_ERR(domain->master,

   195

                "Failed to allocate domain datagram pair!\n");

   196

        return -ENOMEM;

   197

    }

   198

   199

    ret = ec_datagram_pair_init(datagram_pair, domain, logical_offset, data,

   200

            data_size, used);

   201

    if (ret) {

   202

        kfree(datagram_pair);

   203

        return ret;

   204

    }

   205

   206

    domain->expected_working_counter +=

   207

        datagram_pair->expected_working_counter;

   208

   209

    EC_MASTER_DBG(domain->master, 1,

   210

            "Adding datagram pair with expected WC %u.\n",

   211

            datagram_pair->expected_working_counter);

   212

   213

   214

    list_add_tail(&datagram_pair->list, &domain->datagram_pairs);

   215

    return 0;

   216

}

   217

   218

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

   219

   220

/** Domain finish helper function.

   221

 *

   222

 * Detects, if a slave configuration has already been taken into account for

   223

 * a datagram's expected working counter calculation.

   224

 *

   225

 * Walks through the list of all FMMU configurations for the current datagram

   226

 * and ends before the current datagram.

   227

 *

   228

 * \return Non-zero if slave connfig was already counted.

   229

 */

   230

static int shall_count(

   231

        const ec_fmmu_config_t *cur_fmmu, /**< Current FMMU with direction to

   232

                                            search for. */

   233

        const ec_fmmu_config_t *first_fmmu /**< Datagram's first FMMU. */

   234

        )

   235

{

   236

    for (; first_fmmu != cur_fmmu;

   237

            first_fmmu = list_entry(first_fmmu->list.next,

   238

                ec_fmmu_config_t, list)) {

   239

   240

        if (first_fmmu->sc == cur_fmmu->sc

   241

                && first_fmmu->dir == cur_fmmu->dir) {

   242

            return 0; // was already counted

   243

        }

   244

    }

   245

   246

    return 1;

   247

}

   248

   249

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

   250

   251

/** Domain finish helper function.

   252

 *

   253

 * Known boundaries for a datagram have been identified. Scans the datagram

   254

 * FMMU boundaries for WKC counters and then creates the datagram_pair.

   255

 *

   256

 * \param domain The parent domain

   257

 * \param datagram_begin_offset Datagram's logical beginning offset

   258

 * \param datagram_end_offset Logical end offset (one past last byte)

   259

 * \param datagram_first_fmmu The begin FMMU in the datagram

   260

 * \param datagram_end_fmmu The end (one past last) FMMU

   261

 *

   262

 * \return Non-zero if error emplacing domain

   263

 */

   264

 static int emplace_datagram(ec_domain_t *domain,

   265

        uint32_t datagram_begin_offset,

   266

        uint32_t datagram_end_offset,

   267

        const ec_fmmu_config_t *datagram_first_fmmu,

   268

        const ec_fmmu_config_t *datagram_end_fmmu

   269

)

   270

{

   271

    unsigned int datagram_used[EC_DIR_COUNT];

   272

    const ec_fmmu_config_t *curr_fmmu;

   273

    size_t data_size;

   274

   275

    data_size = datagram_end_offset - datagram_begin_offset;

   276

   277

    memset(datagram_used, 0, sizeof(datagram_used));

   278

    for (curr_fmmu = datagram_first_fmmu;

   279

            &curr_fmmu->list != &datagram_end_fmmu->list;

   280

            curr_fmmu = list_next_entry(curr_fmmu, list)) {

   281

        if (shall_count(curr_fmmu, datagram_first_fmmu)) {

   282

            datagram_used[curr_fmmu->dir]++;

   283

        }

   284

    }

   285

   286

    return ec_domain_add_datagram_pair(domain,

   287

            domain->logical_base_address + datagram_begin_offset,

   288

            data_size,

   289

            domain->data + datagram_begin_offset,

   290

            datagram_used);

   291

}

   292

   293

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

   294

   295

/** Finishes a domain.

   296

 *

   297

 * This allocates the necessary datagrams and writes the correct logical

   298

 * addresses to every configured FMMU.

   299

 *

   300

 * \retval  0 Success

   301

 * \retval <0 Error code.

   302

 */

   303

int ec_domain_finish(

   304

        ec_domain_t *domain, /**< EtherCAT domain. */

   305

        uint32_t base_address /**< Logical base address. */

   306

        )

   307

{

   308

    uint32_t datagram_offset = 0;

   309

    unsigned int datagram_count = 0;

   310

    ec_fmmu_config_t *fmmu;

   311

    const ec_fmmu_config_t *datagram_first_fmmu = NULL;

   312

    const ec_fmmu_config_t *valid_fmmu = NULL;

   313

    unsigned candidate_start = 0;

   314

    unsigned valid_start = 0;

   315

    const ec_datagram_pair_t *datagram_pair;

   316

    int ret;

   317

   318

    domain->logical_base_address = base_address;

   319

   320

    if (domain->data_size && domain->data_origin == EC_ORIG_INTERNAL) {

   321

        if (!(domain->data =

   322

                    (uint8_t *) kmalloc(domain->data_size, GFP_KERNEL))) {

   323

            EC_MASTER_ERR(domain->master, "Failed to allocate %zu bytes"

   324

                    " internal memory for domain %u!\n",

   325

                    domain->data_size, domain->index);

   326

            return -ENOMEM;

   327

        }

   328

    }

   329

   330

    // Cycle through all domain FMMUs and

   331

    // - correct the logical base addresses

   332

    // - set up the datagrams to carry the process data

   333

    // - calculate the datagrams' expected working counters

   334

    if (!list_empty(&domain->fmmu_configs)) {

   335

        datagram_first_fmmu =

   336

            list_entry(domain->fmmu_configs.next, ec_fmmu_config_t, list);

   337

    }

   338

   339

    list_for_each_entry(fmmu, &domain->fmmu_configs, list) {

   340

        if (fmmu->data_size > EC_MAX_DATA_SIZE) {

   341

            EC_MASTER_ERR(domain->master,

   342

                "FMMU size %u bytes exceeds maximum data size %u",

   343

                fmmu->data_size, EC_MAX_DATA_SIZE);

   344

            return -EINVAL;

   345

        }

   346

        if (fmmu->logical_domain_offset >= candidate_start) {

   347

            // As FMMU offsets increase monotonically, and candidate start

   348

            // offset has never been contradicted, it can now never be

   349

            // contradicted, as no future FMMU can cross it.

   350

            // All FMMUs prior to this point approved for next datagram

   351

            valid_fmmu = fmmu;

   352

            valid_start = candidate_start;

   353

            if (fmmu->logical_domain_offset + fmmu->data_size - datagram_offset

   354

                    > EC_MAX_DATA_SIZE) {

   355

                // yet the new candidate exceeds the datagram size, so we

   356

                // use the last known valid candidate to create the datagram

   357

                ret = emplace_datagram(domain, datagram_offset, valid_start,

   358

                    datagram_first_fmmu, valid_fmmu);

   359

                if (ret < 0)

   360

                    return ret;

   361

   362

                datagram_offset = valid_start;

   363

                datagram_count++;

   364

                datagram_first_fmmu = fmmu;

   365

            }

   366

        }

   367

        if (fmmu->logical_domain_offset + fmmu->data_size > candidate_start) {

   368

            candidate_start = fmmu->logical_domain_offset + fmmu->data_size;

   369

        }

   370

    }

   371

   372

    /* Allocate last datagram pair, if data are left (this is also the case if

   373

     * the process data fit into a single datagram) */

   374

    if (domain->data_size > datagram_offset) {

   375

        ret = emplace_datagram(domain, datagram_offset, domain->data_size,

   376

            datagram_first_fmmu, fmmu);

   377

        if (ret < 0)

   378

            return ret;

   379

        datagram_count++;

   380

    }

   381

   382

    EC_MASTER_INFO(domain->master, "Domain%u: Logical address 0x%08x,"

   383

            " %zu byte, expected working counter %u.\n", domain->index,

   384

            domain->logical_base_address, domain->data_size,

   385

            domain->expected_working_counter);

   386

   387

    list_for_each_entry(datagram_pair, &domain->datagram_pairs, list) {

   388

        const ec_datagram_t *datagram =

   389

            &datagram_pair->datagrams[EC_DEVICE_MAIN];

   390

        EC_MASTER_INFO(domain->master, "  Datagram %s: Logical offset 0x%08x,"

   391

                " %zu byte, type %s at %p.\n", datagram->name,

   392

                EC_READ_U32(datagram->address), datagram->data_size,

   393

                ec_datagram_type_string(datagram), datagram);

   394

    }

   395

   396

    return 0;

   397

}

   398

   399

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

   400

   401

/** Get the number of FMMU configurations of the domain.

   402

 */

   403

unsigned int ec_domain_fmmu_count(const ec_domain_t *domain)

   404

{

   405

    const ec_fmmu_config_t *fmmu;

   406

    unsigned int num = 0;

   407

   408

    list_for_each_entry(fmmu, &domain->fmmu_configs, list) {

   409

        num++;

   410

    }

   411

   412

    return num;

   413

}

   414

   415

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

   416

   417

/** Get a certain FMMU configuration via its position in the list.

   418

 *

   419

 * \return FMMU at position \a pos, or NULL.

   420

 */

   421

const ec_fmmu_config_t *ec_domain_find_fmmu(

   422

        const ec_domain_t *domain, /**< EtherCAT domain. */

   423

        unsigned int pos /**< List position. */

   424

        )

   425

{

   426

    const ec_fmmu_config_t *fmmu;

   427

   428

    list_for_each_entry(fmmu, &domain->fmmu_configs, list) {

   429

        if (pos--)

   430

            continue;

   431

        return fmmu;

   432

    }

   433

   434

    return NULL;

   435

}

   436

   437

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

   438

   439

#if EC_MAX_NUM_DEVICES > 1

   440

   441

/** Process received data.

   442

 */

   443

int data_changed(

   444

        uint8_t *send_buffer,

   445

        const ec_datagram_t *datagram,

   446

        size_t offset,

   447

        size_t size

   448

        )

   449

{

   450

    uint8_t *sent = send_buffer + offset;

   451

    uint8_t *recv = datagram->data + offset;

   452

    size_t i;

   453

   454

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

   455

        if (recv[i] != sent[i]) {

   456

            return 1;

   457

        }

   458

    }

   459

   460

    return 0;

   461

}

   462

   463

#endif

   464

   465

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

   466

 *  Application interface

   467

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

   468

   469

int ecrt_domain_reg_pdo_entry_list(ec_domain_t *domain,

   470

        const ec_pdo_entry_reg_t *regs)

   471

{

   472

    const ec_pdo_entry_reg_t *reg;

   473

    ec_slave_config_t *sc;

   474

    int ret;

   475

   476

    EC_MASTER_DBG(domain->master, 1, "ecrt_domain_reg_pdo_entry_list("

   477

            "domain = 0x%p, regs = 0x%p)\n", domain, regs);

   478

   479

    for (reg = regs; reg->index; reg++) {

   480

        sc = ecrt_master_slave_config_err(domain->master, reg->alias,

   481

                reg->position, reg->vendor_id, reg->product_code);

   482

        if (IS_ERR(sc))

   483

            return PTR_ERR(sc);

   484

   485

        ret = ecrt_slave_config_reg_pdo_entry(sc, reg->index,

   486

                        reg->subindex, domain, reg->bit_position);

   487

        if (ret < 0)