1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006  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

    10

 *  and/or modify it under the terms of the GNU General Public License

    11

 *  as published by the Free Software Foundation; either version 2 of the

    12

 *  License, or (at your option) any later version.

    13

 *

    14

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

    15

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

    16

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    17

 *  GNU General Public License for more details.

    18

 *

    19

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

    20

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

    21

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

    22

 *

    23

 *  The right to use EtherCAT Technology is granted and comes free of

    24

 *  charge under condition of compatibility of product made by

    25

 *  Licensee. People intending to distribute/sell products based on the

    26

 *  code, have to sign an agreement to guarantee that products using

    27

 *  software based on IgH EtherCAT master stay compatible with the actual

    28

 *  EtherCAT specification (which are released themselves as an open

    29

 *  standard) as the (only) precondition to have the right to use EtherCAT

    30

 *  Technology, IP and trade marks.

    31

 *

    32

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

    33

    34

/**

    35

   \file

    36

   EtherCAT Pdo mapping methods.

    37

*/

    38

    39

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

    40

    41

#include <linux/module.h>

    42

    43

#include "globals.h"

    44

#include "pdo.h"

    45

#include "slave_config.h"

    46

#include "master.h"

    47

    48

#include "pdo_mapping.h"

    49

    50

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

    51

    52

/** Pdo mapping constructor.

    53

 */

    54

void ec_pdo_mapping_init(

    55

        ec_pdo_mapping_t *pm /**< Pdo mapping. */

    56

        )

    57

{

    58

    INIT_LIST_HEAD(&pm->pdos);

    59

    pm->default_mapping = 1;

    60

}

    61

    62

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

    63

    64

/** Pdo mapping destructor.

    65

 */

    66

void ec_pdo_mapping_clear(ec_pdo_mapping_t *pm /**< Pdo mapping. */)

    67

{

    68

    ec_pdo_mapping_clear_pdos(pm);

    69

}

    70

    71

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

    72

    73

/** Clears the list of mapped Pdos.

    74

 */

    75

void ec_pdo_mapping_clear_pdos(ec_pdo_mapping_t *pm /**< Pdo mapping. */)

    76

{

    77

    ec_pdo_t *pdo, *next;

    78

    79

    list_for_each_entry_safe(pdo, next, &pm->pdos, list) {

    80

        list_del_init(&pdo->list);

    81

        ec_pdo_clear(pdo);

    82

        kfree(pdo);

    83

    }

    84

}

    85

    86

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

    87

    88

/** Calculates the total size of the mapped PDO entries.

    89

 *

    90

 * \retval Data size in byte.

    91

 */

    92

uint16_t ec_pdo_mapping_total_size(

    93

        const ec_pdo_mapping_t *pm /**< Pdo mapping. */

    94

        )

    95

{

    96

    unsigned int bit_size;

    97

    const ec_pdo_t *pdo;

    98

    const ec_pdo_entry_t *pdo_entry;

    99

    uint16_t byte_size;

   100

   101

    bit_size = 0;

   102

    list_for_each_entry(pdo, &pm->pdos, list) {

   103

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

   104

            bit_size += pdo_entry->bit_length;

   105

        }

   106

    }

   107

   108

    if (bit_size % 8) // round up to full bytes

   109

        byte_size = bit_size / 8 + 1;

   110

    else

   111

        byte_size = bit_size / 8;

   112

   113

    return byte_size;

   114

}

   115

   116

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

   117

   118

/** Adds a Pdo to the mapping.

   119

 *

   120

 * \return 0 on success, else < 0

   121

 */

   122

int ec_pdo_mapping_add_pdo(

   123

        ec_pdo_mapping_t *pm, /**< Pdo mapping. */

   124

        const ec_pdo_t *pdo /**< PDO to add. */

   125

        )

   126

{

   127

    ec_pdo_t *mapped_pdo;

   128

   129

    // PDO already mapped?

   130

    list_for_each_entry(mapped_pdo, &pm->pdos, list) {

   131

        if (mapped_pdo->index != pdo->index) continue;

   132

        EC_ERR("PDO 0x%04X is already mapped!\n", pdo->index);

   133

        return -1;

   134

    }

   135

   136

    if (!(mapped_pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {

   137

        EC_ERR("Failed to allocate memory for PDO mapping.\n");

   138

        return -1;

   139

    }

   140

   141

    if (ec_pdo_init_copy(mapped_pdo, pdo)) {

   142

        kfree(mapped_pdo);

   143

        return -1;

   144

    }

   145

   146

    list_add_tail(&mapped_pdo->list, &pm->pdos);

   147

    return 0;

   148

}

   149

   150

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

   151

   152

/** Add a Pdo to the mapping.

   153

 *

   154

 * The first call of this method will clear the default mapping.

   155

 *

   156

 * \retval 0 Success.

   157

 * \retval -1 Error.

   158

 */

   159

int ec_pdo_mapping_add_pdo_info(

   160

        ec_pdo_mapping_t *pm, /**< Pdo mapping. */

   161

        const ec_pdo_info_t *pdo_info, /**< Pdo information. */

   162

        const ec_slave_config_t *config /**< Slave configuration, to load

   163

                                          default entries. */

   164

        )

   165

{

   166

    unsigned int i;

   167

    ec_pdo_t *pdo;

   168

    ec_pdo_entry_t *entry;

   169

    const ec_pdo_entry_info_t *entry_info;

   170

   171

    if (pm->default_mapping) {

   172

        pm->default_mapping = 0;

   173

        ec_pdo_mapping_clear_pdos(pm);

   174

    }

   175

   176

    if (!(pdo = (ec_pdo_t *) kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {

   177

        EC_ERR("Failed to allocate memory for Pdo.\n");

   178

        goto out_return;

   179

    }

   180

   181

    ec_pdo_init(pdo);

   182

    pdo->dir = pdo_info->dir;

   183

    pdo->index = pdo_info->index;

   184

   185

    if (config->master->debug_level)

   186

        EC_INFO("Adding Pdo 0x%04X to mapping.\n", pdo->index);

   187

   188

    if (pdo_info->n_entries && pdo_info->entries) { // configuration provided

   189

        if (config->master->debug_level)

   190

            EC_INFO("  Pdo configuration provided.\n");

   191

   192

        for (i = 0; i < pdo_info->n_entries; i++) {

   193

            entry_info = &pdo_info->entries[i];

   194

   195

            if (!(entry = kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) {

   196

                EC_ERR("Failed to allocate memory for PDO entry.\n");

   197

                goto out_free;

   198

            }

   199

   200

            ec_pdo_entry_init(entry);

   201

            entry->index = entry_info->index;

   202

            entry->subindex = entry_info->subindex;

   203

            entry->bit_length = entry_info->bit_length;

   204

            list_add_tail(&entry->list, &pdo->entries);

   205

        }

   206

    } else { // use default Pdo configuration

   207

        if (config->master->debug_level)

   208

            EC_INFO("  Using default Pdo configuration.\n");

   209

   210

        if (config->slave) {

   211

            ec_sync_t *sync;

   212

            ec_pdo_t *default_pdo;

   213

   214

            if ((sync = ec_slave_get_pdo_sync(config->slave, pdo->dir))) {

   215

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

   216

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

   217

                        continue;

   218

                    if (config->master->debug_level)

   219

                        EC_INFO("  Found Pdo name \"%s\".\n",

   220

                                default_pdo->name);

   221

                    // try to take Pdo name from mapped one

   222

                    if (ec_pdo_set_name(pdo, default_pdo->name))

   223

                        goto out_free;

   224

                    // copy entries (= default Pdo configuration)

   225

                    if (ec_pdo_copy_entries(pdo, default_pdo))

   226

                        goto out_free;

   227

                    if (config->master->debug_level) {

   228

                        const ec_pdo_entry_t *entry;

   229

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

   230

                            EC_INFO("    Entry 0x%04X:%u.\n",

   231

                                    entry->index, entry->subindex);

   232

                        }

   233

                    }

   234

                }

   235

            } else {

   236

                EC_WARN("Slave %u does not provide a default Pdo"

   237

                        " configuration!\n", config->slave->ring_position);

   238

            }

   239

        } else {

   240

            EC_WARN("Failed to load default Pdo configuration for %u:%u:"

   241

                    " Slave not found.\n", config->alias, config->position);

   242

        }

   243

    }

   244

   245

    list_add_tail(&pdo->list, &pm->pdos);

   246

    return 0;

   247

   248

out_free:

   249

    ec_pdo_clear(pdo);

   250

    kfree(pdo);

   251

out_return:

   252

    return -1;

   253

}

   254

   255

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

   256

   257

/** Makes a deep copy of another Pdo mapping.

   258

 *

   259

 * \return 0 on success, else < 0

   260

 */

   261

int ec_pdo_mapping_copy(

   262

        ec_pdo_mapping_t *pm, /**< Pdo mapping. */

   263

        const ec_pdo_mapping_t *other /**< PDO mapping to copy from. */

   264

        )

   265

{

   266

    ec_pdo_t *other_pdo;

   267

   268

    ec_pdo_mapping_clear_pdos(pm);

   269

   270

    // PDO already mapped?

   271

    list_for_each_entry(other_pdo, &other->pdos, list) {

   272

        if (ec_pdo_mapping_add_pdo(pm, other_pdo))

   273

            return -1;

   274

    }

   275

   276

    return 0;

   277

}

   278

   279

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

   280

   281

/** Compares two Pdo mappings.

   282

 *

   283

 * Only the mapping is compared, not the Pdo entries (i. e. the Pdo

   284

 * configuration).

   285

 *

   286

 * \retval 1 The given Pdo mappings are equal.

   287

 * \retval 0 The given Pdo mappings differ.

   288

 */

   289

int ec_pdo_mapping_equal(

   290

        const ec_pdo_mapping_t *pm1, /**< First mapping. */

   291

        const ec_pdo_mapping_t *pm2 /**< Second mapping. */

   292

        )

   293

{

   294

    const struct list_head *h1, *h2, *l1, *l2;

   295

    const ec_pdo_t *p1, *p2;

   296

   297

    h1 = l1 = &pm1->pdos;

   298

    h2 = l2 = &pm2->pdos;

   299

   300

    while (1) {

   301

        l1 = l1->next;

   302

        l2 = l2->next;

   303

   304

        if ((l1 == h1) ^ (l2 == h2)) // unequal lengths

   305

            return 0;

   306

        if (l1 == h1 && l2 == h2) // both finished

   307

            break;

   308

   309

        p1 = list_entry(l1, ec_pdo_t, list);

   310

        p2 = list_entry(l2, ec_pdo_t, list);

   311

   312

        if (p1->index != p2->index)

   313

            return 0;

   314

    }

   315

   316

    return 1;

   317

}

   318

   319

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

   320

   321

/** Finds a Pdo with the given index.

   322

 */

   323

const ec_pdo_t *ec_pdo_mapping_find_pdo(

   324

        const ec_pdo_mapping_t *pm, /**< Pdo mapping. */

   325

        uint16_t index /**< Pdo index. */

   326

        )

   327

{

   328

    ec_pdo_t *pdo;

   329

   330

    list_for_each_entry(pdo, &pm->pdos, list) {

   331

        if (pdo->index != index)

   332

            continue;

   333

        return pdo;

   334

    }

   335

   336

    return NULL;

   337

}

   338

   339

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