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

    84

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

    85

    86

/** Domain destructor.

    87

 */

    88

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

    89

{

    90

    ec_datagram_pair_t *datagram_pair, *next_pair;

    91

    92

    // dequeue and free datagrams

    93

    list_for_each_entry_safe(datagram_pair, next_pair,

    94

            &domain->datagram_pairs, list) {

    95

        ec_datagram_pair_clear(datagram_pair);

    96

        kfree(datagram_pair);

    97

    }

    98

    99

    ec_domain_clear_data(domain);

   100

}

   101

   102

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

   103

   104

/** Frees internally allocated memory.

   105

 */

   106

void ec_domain_clear_data(

   107

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

   108

        )

   109

{

   110

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

   111

        kfree(domain->data);

   112

    }

   113

   114

    domain->data = NULL;

   115

    domain->data_origin = EC_ORIG_INTERNAL;

   116

}

   117

   118

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

   119

   120

/** Adds an FMMU configuration to the domain.

   121

 */

   122

void ec_domain_add_fmmu_config(

   123

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

   124

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

   125

        )

   126

{

   127

    fmmu->domain = domain;

   128

   129

    domain->data_size += fmmu->data_size;

   130

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

   131

   132

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

   133

            " Added %u bytes, total %zu.\n",

   134

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

   135

}

   136

   137

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

   138

   139

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

   140

 *

   141

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

   142

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

   143

 *

   144

 * \retval  0 Success.

   145

 * \retval <0 Error code.

   146

 */

   147

int ec_domain_add_datagram_pair(

   148

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

   149

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

   150

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

   151

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

   152

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

   153

        )

   154

{

   155

    ec_datagram_pair_t *datagram_pair;

   156

    int ret;

   157

   158

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

   159

        EC_MASTER_ERR(domain->master,

   160

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

   161

        return -ENOMEM;

   162

    }

   163

   164

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

   165

            data_size, used);

   166

    if (ret) {

   167

        kfree(datagram_pair);

   168

        return ret;

   169

    }

   170

   171

    domain->expected_working_counter +=

   172

        datagram_pair->expected_working_counter;

   173

   174

    EC_MASTER_DBG(domain->master, 1,

   175

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

   176

            datagram_pair->expected_working_counter);

   177

   178

   179

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

   180

    return 0;

   181

}

   182

   183

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

   184

   185

/** Domain finish helper function.

   186

 *

   187

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

   188

 * a datagram's expected working counter calculation.

   189

 *

   190

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

   191

 * and ends before the current datagram.

   192

 *

   193

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

   194

 */

   195

int shall_count(

   196

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

   197

                                            search for. */

   198

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

   199

        )

   200

{

   201

    for (; first_fmmu != cur_fmmu;

   202

            first_fmmu = list_entry(first_fmmu->list.next,

   203

                ec_fmmu_config_t, list)) {

   204

   205

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

   206

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

   207

            return 0; // was already counted

   208

        }

   209

    }

   210

   211

    return 1;

   212

}

   213

   214

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

   215

   216

/** Finishes a domain.

   217

 *

   218

 * This allocates the necessary datagrams and writes the correct logical

   219

 * addresses to every configured FMMU.

   220

 *

   221

 * \todo Check for FMMUs that do not fit into any datagram.

   222

 *

   223

 * \retval  0 Success

   224

 * \retval <0 Error code.

   225

 */

   226

int ec_domain_finish(

   227

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

   228

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

   229

        )

   230

{

   231

    uint32_t datagram_offset;

   232

    size_t datagram_size;

   233

    unsigned int datagram_count;

   234

    unsigned int datagram_used[EC_DIR_COUNT];

   235

    ec_fmmu_config_t *fmmu;

   236

    const ec_fmmu_config_t *datagram_first_fmmu = NULL;

   237

    const ec_datagram_pair_t *datagram_pair;

   238

    int ret;

   239

   240

    domain->logical_base_address = base_address;

   241

   242

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

   243

        if (!(domain->data =

   244

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

   245

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

   246

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

   247

                    domain->data_size, domain->index);

   248

            return -ENOMEM;

   249

        }

   250

    }

   251

   252

    // Cycle through all domain FMMUs and

   253

    // - correct the logical base addresses

   254

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

   255

    // - calculate the datagrams' expected working counters

   256

    datagram_offset = 0;

   257

    datagram_size = 0;

   258

    datagram_count = 0;

   259

    datagram_used[EC_DIR_OUTPUT] = 0;

   260

    datagram_used[EC_DIR_INPUT] = 0;

   261

   262

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

   263

        datagram_first_fmmu =

   264

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

   265

    }

   266

   267

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

   268

   269

        // Correct logical FMMU address

   270

        fmmu->logical_start_address += base_address;

   271

   272

        // Increment Input/Output counter to determine datagram types

   273

        // and calculate expected working counters

   274

        if (shall_count(fmmu, datagram_first_fmmu)) {

   275

            datagram_used[fmmu->dir]++;

   276

        }

   277

   278

        // If the current FMMU's data do not fit in the current datagram,

   279

        // allocate a new one.

   280

        if (datagram_size + fmmu->data_size > EC_MAX_DATA_SIZE) {

   281

            ret = ec_domain_add_datagram_pair(domain,

   282

                    domain->logical_base_address + datagram_offset,

   283

                    datagram_size, domain->data + datagram_offset,

   284

                    datagram_used);

   285

            if (ret < 0)

   286

                return ret;

   287

   288

            datagram_offset += datagram_size;

   289

            datagram_size = 0;

   290

            datagram_count++;

   291

            datagram_used[EC_DIR_OUTPUT] = 0;

   292

            datagram_used[EC_DIR_INPUT] = 0;

   293

            datagram_first_fmmu = fmmu;

   294

        }

   295

   296

        datagram_size += fmmu->data_size;

   297

    }

   298

   299

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

   300

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

   301

    if (datagram_size) {

   302

        ret = ec_domain_add_datagram_pair(domain,

   303

                domain->logical_base_address + datagram_offset,

   304

                datagram_size, domain->data + datagram_offset,

   305

                datagram_used);

   306

        if (ret < 0)

   307

            return ret;

   308

        datagram_count++;

   309

    }

   310

   311

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

   312

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

   313

            domain->logical_base_address, domain->data_size,

   314

            domain->expected_working_counter);

   315

   316

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

   317

        const ec_datagram_t *datagram =

   318

            &datagram_pair->datagrams[EC_DEVICE_MAIN];

   319

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

   320

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

   321

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

   322

                ec_datagram_type_string(datagram));

   323

    }

   324

   325

    return 0;

   326

}

   327

   328

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

   329

   330

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

   331

 */

   332

unsigned int ec_domain_fmmu_count(const ec_domain_t *domain)

   333

{

   334

    const ec_fmmu_config_t *fmmu;

   335

    unsigned int num = 0;

   336

   337

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

   338

        num++;

   339

    }

   340

   341

    return num;

   342

}

   343

   344

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

   345

   346

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

   347

 *

   348

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

   349

 */

   350

const ec_fmmu_config_t *ec_domain_find_fmmu(

   351

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

   352

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

   353

        )

   354

{

   355

    const ec_fmmu_config_t *fmmu;

   356

   357

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

   358

        if (pos--)

   359

            continue;

   360

        return fmmu;

   361

    }

   362

   363

    return NULL;

   364

}

   365

   366

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

   367

   368

#if EC_MAX_NUM_DEVICES > 1

   369

   370

/** Process received data.

   371

 */

   372

int data_changed(

   373

        uint8_t *send_buffer,

   374

        const ec_datagram_t *datagram,

   375

        size_t offset,

   376

        size_t size

   377

        )

   378

{

   379

    uint8_t *sent = send_buffer + offset;

   380

    uint8_t *recv = datagram->data + offset;

   381

    size_t i;

   382

   383

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

   384

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

   385

            return 1;

   386

        }

   387

    }

   388

   389

    return 0;

   390

}

   391

   392

#endif

   393

   394

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

   395

 *  Application interface

   396

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

   397

   398

int ecrt_domain_reg_pdo_entry_list(ec_domain_t *domain,

   399

        const ec_pdo_entry_reg_t *regs)

   400

{

   401

    const ec_pdo_entry_reg_t *reg;

   402

    ec_slave_config_t *sc;

   403

    int ret;

   404

   405

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

   406

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

   407

   408

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

   409

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

   410

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

   411

        if (IS_ERR(sc))

   412

            return PTR_ERR(sc);

   413

   414

        ret = ecrt_slave_config_reg_pdo_entry(sc, reg->index,

   415

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

   416

        if (ret < 0)

   417

            return ret;

   418

   419

        *reg->offset = ret;

   420

    }

   421

   422

    return 0;

   423

}

   424

   425

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

   426

   427

size_t ecrt_domain_size(const ec_domain_t *domain)

   428

{

   429

    return domain->data_size;

   430

}

   431

   432

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

   433

   434

void ecrt_domain_external_memory(ec_domain_t *domain, uint8_t *mem)

   435

{

   436

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

   437

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

   438

   439

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

   440

   441

    ec_domain_clear_data(domain);

   442

   443

    domain->data = mem;

   444

    domain->data_origin = EC_ORIG_EXTERNAL;

   445

   446

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

   447

}

   448

   449

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

   450

   451

uint8_t *ecrt_domain_data(ec_domain_t *domain)

   452

{

   453

    return domain->data;

   454

}

   455

   456

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

   457

   458

void ecrt_domain_process(ec_domain_t *domain)

   459

{

   460

    uint16_t wc_sum[EC_MAX_NUM_DEVICES] = {}, wc_total;

   461

    ec_datagram_pair_t *pair;

   462

#if EC_MAX_NUM_DEVICES > 1

   463

    uint16_t datagram_pair_wc, redundant_wc;

   464

    unsigned int datagram_offset;

   465

    ec_fmmu_config_t *fmmu = list_first_entry(&domain->fmmu_configs,

   466

            ec_fmmu_config_t, list);

   467

    unsigned int redundancy;

   468

#endif

   469

    unsigned int dev_idx, wc_change;

   470

   471

#if DEBUG_REDUNDANCY

   472

    EC_MASTER_DBG(domain->master, 1, "domain %u process\n", domain->index);

   473

#endif

   474

   475

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

   476

#if EC_MAX_NUM_DEVICES > 1

   477

        datagram_pair_wc = ec_datagram_pair_process(pair, wc_sum);

   478

#else

   479

        ec_datagram_pair_process(pair, wc_sum);

   480

#endif

   481

   482

#if EC_MAX_NUM_DEVICES > 1

   483

        if (ec_master_num_devices(domain->master) > 1) {

   484

            ec_datagram_t *main_datagram = &pair->datagrams[EC_DEVICE_MAIN];

   485

            uint32_t logical_datagram_address =

   486