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

#ifndef list_next_entry

    51

#define list_next_entry(pos, member) \

    52

    list_entry((pos)->member.next, typeof(*(pos)), member)

    53

#endif

    54

    55

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

    56

    57

void ec_domain_clear_data(ec_domain_t *);

    58

    59

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

    60

    61

/** Domain constructor.

    62

 */

    63

void ec_domain_init(

    64

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

    65

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

    66

        unsigned int index /**< Index. */

    67

        )

    68

{

    69

    unsigned int dev_idx;

    70

    71

    domain->master = master;

    72

    domain->index = index;

    73

    INIT_LIST_HEAD(&domain->fmmu_configs);

    74

    domain->data_size = 0;

    75

    domain->data = NULL;

    76

    domain->data_origin = EC_ORIG_INTERNAL;

    77

    domain->logical_base_address = 0x00000000;

    78

    INIT_LIST_HEAD(&domain->datagram_pairs);

    79

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

    80

            dev_idx++) {

    81

        domain->working_counter[dev_idx] = 0x0000;

    82

    }

    83

    domain->expected_working_counter = 0x0000;

    84

    domain->working_counter_changes = 0;

    85

    domain->redundancy_active = 0;

    86

    domain->notify_jiffies = 0;

    87

    88

    /* Used by ec_domain_add_fmmu_config */

    89

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

    90

    domain->sc_in_work = 0;

    91

}

    92

    93

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

    94

    95

/** Domain destructor.

    96

 */

    97

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

    98

{

    99

    ec_datagram_pair_t *datagram_pair, *next_pair;

   100

   101

    // dequeue and free datagrams

   102

    list_for_each_entry_safe(datagram_pair, next_pair,

   103

            &domain->datagram_pairs, list) {

   104

        ec_datagram_pair_clear(datagram_pair);

   105

        kfree(datagram_pair);

   106

    }

   107

   108

    ec_domain_clear_data(domain);

   109

}

   110

   111

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

   112

   113

/** Frees internally allocated memory.

   114

 */

   115

void ec_domain_clear_data(

   116

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

   117

        )

   118

{

   119

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

   120

        kfree(domain->data);

   121

    }

   122

   123

    domain->data = NULL;

   124

    domain->data_origin = EC_ORIG_INTERNAL;

   125

}

   126

   127

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

   128

   129

/** Adds an FMMU configuration to the domain.

   130

 */

   131

void ec_domain_add_fmmu_config(

   132

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

   133

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

   134

        )

   135

{

   136

    const ec_slave_config_t *sc;

   137

    uint32_t logical_domain_offset;

   138

    unsigned fmmu_data_size;

   139

   140

    fmmu->domain = domain;

   141

    sc = fmmu->sc;

   142

   143

    fmmu_data_size = ec_pdo_list_total_size(

   144

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

   145

   146

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

   147

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

   148

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

   149

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

   150

    } else {

   151

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

   152

        // FMMU on this domain.

   153

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

   154

            domain->offset_used[EC_DIR_OUTPUT]);

   155

        // rebase the free offsets to the current position

   156

        domain->offset_used[EC_DIR_INPUT] = logical_domain_offset;

   157

        domain->offset_used[EC_DIR_OUTPUT] = logical_domain_offset;

   158

    }

   159

    domain->sc_in_work = sc;

   160

   161

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

   162

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

   163

   164

    ec_fmmu_set_domain_offset_size(fmmu, logical_domain_offset, fmmu_data_size);

   165

   166

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

   167

   168

    // Determine domain size from furthest extent of FMMU data

   169

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

   170

            domain->offset_used[EC_DIR_OUTPUT]);

   171

   172

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

   173

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

   174

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

   175

}

   176

   177

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

   178

   179

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

   180

 *

   181

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

   182

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

   183

 *

   184

 * \retval  0 Success.

   185

 * \retval <0 Error code.

   186

 */

   187

int ec_domain_add_datagram_pair(

   188

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

   189

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

   190

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

   191

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

   192

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

   193

        )

   194

{

   195

    ec_datagram_pair_t *datagram_pair;

   196

    int ret;

   197

   198

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

   199

        EC_MASTER_ERR(domain->master,

   200

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

   201

        return -ENOMEM;

   202

    }

   203

   204

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

   205

            data_size, used);

   206

    if (ret) {

   207

        kfree(datagram_pair);

   208

        return ret;

   209

    }

   210

   211

    domain->expected_working_counter +=

   212

        datagram_pair->expected_working_counter;

   213

   214

    EC_MASTER_DBG(domain->master, 1,

   215

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

   216

            datagram_pair->expected_working_counter);

   217

   218

   219

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

   220

    return 0;

   221

}

   222

   223

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

   224

   225

/** Domain finish helper function.

   226

 *

   227

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

   228

 * a datagram's expected working counter calculation.

   229

 *

   230

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

   231

 * and ends before the current datagram.

   232

 *

   233

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

   234

 */

   235

static int shall_count(

   236

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

   237

                                            search for. */

   238

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

   239

        )

   240

{

   241

    for (; first_fmmu != cur_fmmu;

   242

            first_fmmu = list_entry(first_fmmu->list.next,

   243

                ec_fmmu_config_t, list)) {

   244

   245

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

   246

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

   247

            return 0; // was already counted

   248

        }

   249

    }

   250

   251

    return 1;

   252

}

   253

   254

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

   255

   256

/** Domain finish helper function.

   257

 *

   258

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

   259

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

   260

 *

   261

 * \param domain The parent domain

   262

 * \param datagram_begin_offset Datagram's logical beginning offset

   263

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

   264

 * \param datagram_first_fmmu The begin FMMU in the datagram

   265

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

   266

 *

   267

 * \return Non-zero if error emplacing domain

   268

 */

   269

static int emplace_datagram(ec_domain_t *domain,

   270

        uint32_t datagram_begin_offset,

   271

        uint32_t datagram_end_offset,

   272

        const ec_fmmu_config_t *datagram_first_fmmu,

   273

        const ec_fmmu_config_t *datagram_end_fmmu

   274

        )

   275

{

   276

    unsigned int datagram_used[EC_DIR_COUNT];

   277

    const ec_fmmu_config_t *curr_fmmu;

   278

    size_t data_size;

   279

   280

    data_size = datagram_end_offset - datagram_begin_offset;

   281

   282

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

   283

    for (curr_fmmu = datagram_first_fmmu;

   284

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

   285

            curr_fmmu = list_next_entry(curr_fmmu, list)) {

   286

        if (shall_count(curr_fmmu, datagram_first_fmmu)) {

   287

            datagram_used[curr_fmmu->dir]++;

   288

        }

   289

    }

   290

   291

    return ec_domain_add_datagram_pair(domain,

   292

            domain->logical_base_address + datagram_begin_offset,

   293

            data_size,

   294

            domain->data + datagram_begin_offset,

   295

            datagram_used);

   296

}

   297

   298

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

   299

   300

/** Finishes a domain.

   301

 *

   302

 * This allocates the necessary datagrams and writes the correct logical

   303

 * addresses to every configured FMMU.

   304

 *

   305

 * \retval  0 Success

   306

 * \retval <0 Error code.

   307

 */

   308

int ec_domain_finish(

   309

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

   310

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

   311

        )

   312

{

   313

    uint32_t datagram_offset = 0;

   314

    unsigned int datagram_count = 0;

   315

    ec_fmmu_config_t *fmmu;

   316

    const ec_fmmu_config_t *datagram_first_fmmu = NULL;

   317

    const ec_fmmu_config_t *valid_fmmu = NULL;

   318

    unsigned candidate_start = 0;

   319

    unsigned valid_start = 0;

   320

    const ec_datagram_pair_t *datagram_pair;

   321

    int ret;

   322

   323

    domain->logical_base_address = base_address;

   324

   325

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

   326

        if (!(domain->data =

   327

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

   328

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

   329

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

   330

                    domain->data_size, domain->index);

   331

            return -ENOMEM;

   332

        }

   333

    }

   334

   335

    // Cycle through all domain FMMUs and

   336

    // - correct the logical base addresses

   337

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

   338

    // - calculate the datagrams' expected working counters

   339

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

   340

        datagram_first_fmmu =

   341

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

   342

    }

   343

   344

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

   345

        if (fmmu->data_size > EC_MAX_DATA_SIZE) {

   346

            EC_MASTER_ERR(domain->master,

   347

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

   348

                fmmu->data_size, EC_MAX_DATA_SIZE);

   349

            return -EINVAL;

   350

        }

   351

        if (fmmu->logical_domain_offset >= candidate_start) {

   352

            // As FMMU offsets increase monotonically, and candidate start

   353

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

   354

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

   355

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

   356

            valid_fmmu = fmmu;

   357

            valid_start = candidate_start;

   358

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

   359

                    > EC_MAX_DATA_SIZE) {

   360

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

   361

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

   362

                ret = emplace_datagram(domain, datagram_offset, valid_start,

   363

                    datagram_first_fmmu, valid_fmmu);

   364

                if (ret < 0)

   365

                    return ret;

   366

   367

                datagram_offset = valid_start;

   368

                datagram_count++;

   369

                datagram_first_fmmu = fmmu;

   370

            }

   371

        }

   372

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

   373

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

   374

        }

   375

    }

   376

   377

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

   378

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

   379

    if (domain->data_size > datagram_offset) {

   380

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

   381

            datagram_first_fmmu, fmmu);

   382

        if (ret < 0)

   383

            return ret;

   384

        datagram_count++;

   385

    }

   386

   387

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

   388

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

   389

            domain->logical_base_address, domain->data_size,

   390

            domain->expected_working_counter);

   391

   392

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

   393

        const ec_datagram_t *datagram =

   394

            &datagram_pair->datagrams[EC_DEVICE_MAIN];

   395

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

   396

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

   397

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

   398

                ec_datagram_type_string(datagram), datagram);

   399

    }

   400

   401

    return 0;

   402

}

   403

   404

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

   405

   406

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

   407

 */

   408

unsigned int ec_domain_fmmu_count(const ec_domain_t *domain)

   409

{

   410

    const ec_fmmu_config_t *fmmu;

   411

    unsigned int num = 0;

   412

   413

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

   414

        num++;

   415

    }

   416

   417

    return num;

   418

}

   419

   420

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

   421

   422

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

   423

 *

   424

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

   425

 */

   426

const ec_fmmu_config_t *ec_domain_find_fmmu(

   427

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

   428

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

   429

        )

   430

{

   431

    const ec_fmmu_config_t *fmmu;

   432

   433

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

   434

        if (pos--)

   435

            continue;

   436

        return fmmu;

   437

    }

   438

   439

    return NULL;

   440

}

   441

   442

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

   443

   444

#if EC_MAX_NUM_DEVICES > 1

   445

   446

/** Process received data.

   447

 */

   448

int data_changed(

   449

        uint8_t *send_buffer,

   450

        const ec_datagram_t *datagram,

   451

        size_t offset,

   452

        size_t size

   453

        )

   454

{

   455

    uint8_t *sent = send_buffer + offset;

   456

    uint8_t *recv = datagram->data + offset;

   457

    size_t i;

   458

   459

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

   460

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

   461

            return 1;

   462

        }

   463

    }

   464

   465

    return 0;

   466

}

   467

   468

#endif

   469

   470

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

   471

 *  Application interface

   472

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

   473

   474

int ecrt_domain_reg_pdo_entry_list(ec_domain_t *domain,

   475

        const ec_pdo_entry_reg_t *regs)

   476

{

   477

    const ec_pdo_entry_reg_t *reg;

   478

    ec_slave_config_t *sc;

   479

    int ret;

   480

   481

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

   482

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

   483

   484

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

   485

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

   486

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

   487

        if (IS_ERR(sc))