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

/** Add a new Pdo to the mapping.

   119

 *

   120

 * \retval >0 Pointer to new Pdo.

   121

 * \retval NULL No memory.

   122

 */

   123

ec_pdo_t *ec_pdo_mapping_add_pdo(

   124

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

   127

        )

   128

{

   129

    ec_pdo_t *pdo;

   130

   131

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

   132

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

   133

        return NULL;

   134

    }

   135

   136

    ec_pdo_init(pdo);

   137

    pdo->dir = dir;

   138

    pdo->index = index;

   139

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

   140

    return pdo;

   141

}

   142

   143

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

   144

   145

/** Add the copy of an existing Pdo to the mapping.

   146

 *

   147

 * \return 0 on success, else < 0

   148

 */

   149

int ec_pdo_mapping_add_pdo_copy(

   150

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

   151

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

   152

        )

   153

{

   154

    ec_pdo_t *mapped_pdo;

   155

   156

    // Pdo already mapped?

   157

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

   158

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

   159

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

   160

        return -1;

   161

    }

   162

   163

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

   164

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

   165

        return -1;

   166

    }

   167

   168

    if (ec_pdo_init_copy(mapped_pdo, pdo)) {

   169

        kfree(mapped_pdo);

   170

        return -1;

   171

    }

   172

   173

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

   174

    return 0;

   175

}

   176

   177

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

   178

   179

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

   180

 *

   181

 * \return 0 on success, else < 0

   182

 */

   183

int ec_pdo_mapping_copy(

   184

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

   185

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

   186

        )

   187

{

   188

    ec_pdo_t *other_pdo;

   189

   190

    ec_pdo_mapping_clear_pdos(pm);

   191

   192

    // Pdo already mapped?

   193

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

   194

        if (ec_pdo_mapping_add_pdo_copy(pm, other_pdo))

   195

            return -1;

   196

    }

   197

   198

    return 0;

   199

}

   200

   201

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

   202

   203

/** Compares two Pdo mappings.

   204

 *

   205

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

   206

 * configuration).

   207

 *

   208

 * \retval 1 The given Pdo mappings are equal.

   209

 * \retval 0 The given Pdo mappings differ.

   210

 */

   211

int ec_pdo_mapping_equal(

   212

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

   213

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

   214

        )

   215

{

   216

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

   217

    const ec_pdo_t *p1, *p2;

   218

   219

    h1 = l1 = &pm1->pdos;

   220

    h2 = l2 = &pm2->pdos;

   221

   222

    while (1) {

   223

        l1 = l1->next;

   224

        l2 = l2->next;

   225

   226

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

   227

            return 0;

   228

        if (l1 == h1) // both finished

   229

            break;

   230

   231

        p1 = list_entry(l1, ec_pdo_t, list);

   232

        p2 = list_entry(l2, ec_pdo_t, list);

   233

   234

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

   235

            return 0;

   236

    }

   237

   238

    return 1;

   239

}

   240

   241

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

   242

   243

/** Finds a Pdo with the given index.

   244

 */

   245

ec_pdo_t *ec_pdo_mapping_find_pdo(

   246

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

   247

        uint16_t index /**< Pdo index. */

   248

        )

   249

{

   250

    ec_pdo_t *pdo;

   251

   252

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

   253

        if (pdo->index != index)

   254

            continue;

   255

        return pdo;

   256

    }

   257

   258

    return NULL;

   259

}

   260

   261

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

   262

   263

/** Finds a Pdo with the given index and returns a const pointer.

   264

 */

   265

const ec_pdo_t *ec_pdo_mapping_find_pdo_const(

   266

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

   267

        uint16_t index /**< Pdo index. */

   268

        )

   269

{

   270

    const ec_pdo_t *pdo;

   271

   272

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

   273

        if (pdo->index != index)

   274

            continue;

   275

        return pdo;

   276

    }

   277

   278

    return NULL;

   279

}

   280

   281

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