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

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

    47

    48

void ec_domain_clear_data(ec_domain_t *);

    49

    50

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

    51

    52

/** Domain constructor.

    53

 */

    54

void ec_domain_init(

    55

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

    56

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

    57

        unsigned int index /**< Index. */

    58

        )

    59

{

    60

    domain->master = master;

    61

    domain->index = index;

    62

    INIT_LIST_HEAD(&domain->fmmu_configs);

    63

    domain->data_size = 0;

    64

    domain->data = NULL;

    65

    domain->data_origin = EC_ORIG_INTERNAL;

    66

    domain->logical_base_address = 0x00000000;

    67

    INIT_LIST_HEAD(&domain->datagram_pairs);

    68

    domain->working_counter = 0x0000;

    69

    domain->expected_working_counter = 0x0000;

    70

    domain->working_counter_changes = 0;

    71

    domain->notify_jiffies = 0;

    72

}

    73

    74

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

    75

    76

/** Domain destructor.

    77

 */

    78

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

    79

{

    80

    ec_datagram_pair_t *datagram_pair, *next_pair;

    81

    82

    // dequeue and free datagrams

    83

    list_for_each_entry_safe(datagram_pair, next_pair,

    84

            &domain->datagram_pairs, list) {

    85

        ec_datagram_pair_clear(datagram_pair);

    86

        kfree(datagram_pair);

    87

    }

    88

    89

    ec_domain_clear_data(domain);

    90

}

    91

    92

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

    93

    94

/** Frees internally allocated memory.

    95

 */

    96

void ec_domain_clear_data(

    97

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

    98

        )

    99

{

   100

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

   101

        kfree(domain->data);

   102

    }

   103

   104

    domain->data = NULL;

   105

    domain->data_origin = EC_ORIG_INTERNAL;

   106

}

   107

   108

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

   109

   110

/** Adds an FMMU configuration to the domain.

   111

 */

   112

void ec_domain_add_fmmu_config(

   113

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

   114

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

   115

        )

   116

{

   117

    fmmu->domain = domain;

   118

   119

    domain->data_size += fmmu->data_size;

   120

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

   121

   122

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

   123

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

   124

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

   125

}

   126

   127

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

   128

   129

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

   130

 *

   131

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

   132

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

   133

 *

   134

 * \retval  0 Success.

   135

 * \retval <0 Error code.

   136

 */

   137

int ec_domain_add_datagram_pair(

   138

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

   139

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

   140

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

   141

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

   142

        const unsigned int used[] /**< Used by inputs/outputs. */

   143

        )

   144

{

   145

    ec_datagram_pair_t *datagram_pair;

   146

    int ret;

   147

    unsigned int i;

   148

   149

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

   150

        EC_MASTER_ERR(domain->master,

   151

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

   152

        return -ENOMEM;

   153

    }

   154

   155

    ec_datagram_pair_init(datagram_pair);

   156

   157

    /* backup datagram has its own memory */

   158

    ret = ec_datagram_prealloc(&datagram_pair->datagrams[EC_DEVICE_BACKUP],

   159

            data_size);

   160

    if (ret) {

   161

        ec_datagram_pair_clear(datagram_pair);

   162

        kfree(datagram_pair);

   163

        return ret;

   164

    }

   165

   166

    /* The ec_datagram_lxx() calls below can not fail, because either the

   167

     * datagram has external memory or it is preallocated. */

   168

   169

    if (used[EC_DIR_OUTPUT] && used[EC_DIR_INPUT]) { // inputs and outputs

   170

        ec_datagram_lrw_ext(&datagram_pair->datagrams[EC_DEVICE_MAIN],

   171

                logical_offset, data_size, data);

   172

        ec_datagram_lrw(&datagram_pair->datagrams[EC_DEVICE_BACKUP],

   173

                logical_offset, data_size);

   174

   175

        // If LRW is used, output FMMUs increment the working counter by 2,

   176

        // while input FMMUs increment it by 1.

   177

        domain->expected_working_counter +=

   178

            used[EC_DIR_OUTPUT] * 2 + used[EC_DIR_INPUT];

   179

    } else if (used[EC_DIR_OUTPUT]) { // outputs only

   180

        ec_datagram_lwr_ext(&datagram_pair->datagrams[EC_DEVICE_MAIN],

   181

                logical_offset, data_size, data);

   182

        ec_datagram_lwr(&datagram_pair->datagrams[EC_DEVICE_BACKUP],

   183

                logical_offset, data_size);

   184

   185

        domain->expected_working_counter += used[EC_DIR_OUTPUT];

   186

    } else { // inputs only (or nothing)

   187

        ec_datagram_lrd_ext(&datagram_pair->datagrams[EC_DEVICE_MAIN],

   188

                logical_offset, data_size, data);

   189

        ec_datagram_lrd(&datagram_pair->datagrams[EC_DEVICE_BACKUP],

   190

                logical_offset, data_size);

   191

   192

        domain->expected_working_counter += used[EC_DIR_INPUT];

   193

    }

   194

   195

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

   196

        snprintf(datagram_pair->datagrams[i].name, EC_DATAGRAM_NAME_SIZE,

   197

                "domain%u-%u-%s", domain->index, logical_offset,

   198

                i ? "backup" : "main");

   199

        ec_datagram_zero(&datagram_pair->datagrams[i]);

   200

    }

   201

   202

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

   203

    return 0;

   204

}

   205

   206

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

   207

   208

/** Finishes a domain.

   209

 *

   210

 * This allocates the necessary datagrams and writes the correct logical

   211

 * addresses to every configured FMMU.

   212

 *

   213

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

   214

 *

   215

 * \retval  0 Success

   216

 * \retval <0 Error code.

   217

 */

   218

int ec_domain_finish(

   219

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

   220

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

   221

        )

   222

{

   223

    uint32_t datagram_offset;

   224

    size_t datagram_size;

   225

    unsigned int datagram_count;

   226

    unsigned int datagram_used[EC_DIR_COUNT];

   227

    ec_fmmu_config_t *fmmu;

   228

    ec_fmmu_config_t *fmmu_temp;

   229

    const ec_datagram_pair_t *datagram_pair;

   230

    int ret;

   231

   232

    domain->logical_base_address = base_address;

   233

   234

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

   235

        if (!(domain->data =

   236

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

   237

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

   238

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

   239

                    domain->data_size, domain->index);

   240

            return -ENOMEM;

   241

        }

   242

    }

   243

   244

    // Cycle through all domain FMMUS and

   245

    // - correct the logical base addresses

   246

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

   247

    datagram_offset = 0;

   248

    datagram_size = 0;

   249

    datagram_count = 0;

   250

    datagram_used[EC_DIR_OUTPUT] = 0;

   251

    datagram_used[EC_DIR_INPUT] = 0;

   252

   253

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

   254

        // we have to remove the constness, sorry FIXME

   255

        ec_slave_config_t *sc = (ec_slave_config_t *) fmmu_temp->sc;

   256

        sc->used_for_fmmu_datagram[fmmu_temp->dir] = 0;

   257

    }

   258

   259

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

   260

        // Correct logical FMMU address

   261

        fmmu->logical_start_address += base_address;

   262

   263

        // Increment Input/Output counter to determine datagram types

   264

        // and calculate expected working counters

   265

        if (fmmu->sc->used_for_fmmu_datagram[fmmu->dir] == 0) {

   266

            ec_slave_config_t *sc = (ec_slave_config_t *)fmmu->sc;

   267

            datagram_used[fmmu->dir]++;

   268

            sc->used_for_fmmu_datagram[fmmu->dir] = 1;

   269

        }

   270

   271

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

   272

        // allocate a new one.

   273

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

   274

            ret = ec_domain_add_datagram_pair(domain,

   275

                    domain->logical_base_address + datagram_offset,

   276

                    datagram_size, domain->data + datagram_offset,

   277

                    datagram_used);

   278

            if (ret < 0)

   279

                return ret;

   280

            datagram_offset += datagram_size;

   281

            datagram_size = 0;

   282

            datagram_count++;

   283

            datagram_used[EC_DIR_OUTPUT] = 0;

   284

            datagram_used[EC_DIR_INPUT] = 0;

   285

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

   286

                ec_slave_config_t *sc = (ec_slave_config_t *)fmmu_temp->sc;

   287

               sc->used_for_fmmu_datagram[fmmu_temp->dir] = 0;

   288

            }

   289

        }

   290

   291

        datagram_size += fmmu->data_size;

   292

    }

   293

   294

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

   295

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

   296

    if (datagram_size) {

   297

        ret = ec_domain_add_datagram_pair(domain,

   298

                domain->logical_base_address + datagram_offset,

   299

                datagram_size, domain->data + datagram_offset,

   300

                datagram_used);

   301

        if (ret < 0)

   302

            return ret;

   303

        datagram_count++;

   304

    }

   305

   306

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

   307

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

   308

            domain->logical_base_address, domain->data_size,

   309

            domain->expected_working_counter);

   310

   311

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

   312

        const ec_datagram_t *datagram =

   313

            &datagram_pair->datagrams[EC_DEVICE_MAIN];

   314

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

   315

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

   316

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

   317

                ec_datagram_type_string(datagram));

   318

    }

   319

   320

    return 0;

   321

}

   322

   323

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

   324

   325

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

   326

 */

   327

unsigned int ec_domain_fmmu_count(const ec_domain_t *domain)

   328

{

   329

    const ec_fmmu_config_t *fmmu;

   330

    unsigned int num = 0;

   331

   332

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

   333

        num++;

   334

    }

   335

   336

    return num;

   337

}

   338

   339

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

   340

   341

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

   342

 */

   343

const ec_fmmu_config_t *ec_domain_find_fmmu(

   344

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

   345

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

   346

        )

   347

{

   348

    const ec_fmmu_config_t *fmmu;

   349

   350

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

   351

        if (pos--)

   352

            continue;

   353

        return fmmu;

   354

    }

   355

   356

    return NULL;

   357

}

   358

   359

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

   360

 *  Application interface

   361

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

   362

   363

int ecrt_domain_reg_pdo_entry_list(ec_domain_t *domain,

   364

        const ec_pdo_entry_reg_t *regs)

   365

{

   366

    const ec_pdo_entry_reg_t *reg;

   367

    ec_slave_config_t *sc;

   368

    int ret;

   369

   370

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

   371

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

   372

   373

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

   374

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

   375

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

   376

        if (IS_ERR(sc))

   377

            return PTR_ERR(sc);

   378

   379

        ret = ecrt_slave_config_reg_pdo_entry(sc, reg->index,

   380

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

   381

        if (ret < 0)

   382

            return ret;

   383

   384

        *reg->offset = ret;

   385

    }

   386

   387

    return 0;

   388

}

   389

   390

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

   391

   392

size_t ecrt_domain_size(const ec_domain_t *domain)

   393

{

   394

    return domain->data_size;

   395

}

   396

   397

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

   398

   399

void ecrt_domain_external_memory(ec_domain_t *domain, uint8_t *mem)

   400

{

   401

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

   402

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

   403

   404

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

   405

   406

    ec_domain_clear_data(domain);

   407

   408

    domain->data = mem;

   409

    domain->data_origin = EC_ORIG_EXTERNAL;

   410

   411

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

   412

}

   413

   414

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

   415

   416

uint8_t *ecrt_domain_data(ec_domain_t *domain)

   417

{

   418

    return domain->data;

   419

}

   420

   421

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

   422

   423

void ecrt_domain_process(ec_domain_t *domain)

   424

{

   425

    uint16_t working_counter_sum;

   426

    ec_datagram_pair_t *datagram_pair;

   427

    unsigned int i;

   428

   429

    working_counter_sum = 0;

   430

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

   431

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

   432

            ec_datagram_t *datagram = &datagram_pair->datagrams[i];

   433

            ec_datagram_output_stats(datagram);

   434

            if (datagram->state == EC_DATAGRAM_RECEIVED) {

   435

                working_counter_sum += datagram->working_counter;

   436

            }

   437

        }

   438

    }

   439

   440

    if (working_counter_sum != domain->working_counter) {

   441

        domain->working_counter_changes++;

   442

        domain->working_counter = working_counter_sum;

   443

    }

   444

   445

    if (domain->working_counter_changes &&

   446

        jiffies - domain->notify_jiffies > HZ) {

   447

        domain->notify_jiffies = jiffies;

   448

        if (domain->working_counter_changes == 1) {

   449

            EC_MASTER_INFO(domain->master, "Domain %u: Working counter"

   450

                    " changed to %u/%u.\n", domain->index,

   451

                    domain->working_counter, domain->expected_working_counter);

   452

        } else {

   453

            EC_MASTER_INFO(domain->master, "Domain %u: %u working counter"

   454

                    " changes - now %u/%u.\n", domain->index,

   455

                    domain->working_counter_changes, domain->working_counter,

   456

                    domain->expected_working_counter);

   457

        }

   458

        domain->working_counter_changes = 0;

   459

    }

   460

}

   461

   462

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

   463

   464

void ecrt_domain_queue(ec_domain_t *domain)

   465

{

   466

    ec_datagram_pair_t *datagram_pair;

   467

    unsigned int i;

   468

   469

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

   470

   471

        /* copy main data to backup datagram */

   472

        memcpy(datagram_pair->datagrams[EC_DEVICE_BACKUP].data,

   473

                datagram_pair->datagrams[EC_DEVICE_MAIN].data,

   474

                datagram_pair->datagrams[EC_DEVICE_MAIN].data_size);

   475

   476

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

   477

            ec_master_queue_datagram(domain->master,

   478

                    &datagram_pair->datagrams[i], i);

   479

        }

   480

    }

   481

}

   482

   483

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

   484

   485

void ecrt_domain_state(const ec_domain_t *domain, ec_domain_state_t *state)