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 process data object methods.

    37

*/

    38

    39

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

    40

    41

#include <linux/slab.h>

    42

    43

#include "pdo.h"

    44

    45

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

    46

    47

/** Pdo constructor.

    48

 */

    49

void ec_pdo_init(

    50

        ec_pdo_t *pdo /**< EtherCAT Pdo */

    51

        )

    52

{

    53

    pdo->sync_index = -1; // not assigned 

    54

    pdo->name = NULL;

    55

    INIT_LIST_HEAD(&pdo->entries);

    56

    pdo->default_config = 0;

    57

}

    58

    59

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

    60

    61

/** Pdo copy constructor.

    62

 */

    63

int ec_pdo_init_copy(ec_pdo_t *pdo, const ec_pdo_t *other_pdo)

    64

{

    65

    pdo->dir = other_pdo->dir;

    66

    pdo->index = other_pdo->index;

    67

    pdo->sync_index = other_pdo->sync_index;

    68

    pdo->name = NULL;

    69

    INIT_LIST_HEAD(&pdo->entries);

    70

    pdo->default_config = other_pdo->default_config;

    71

    72

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

    73

        goto out_return;

    74

    75

    if (ec_pdo_copy_entries(pdo, other_pdo))

    76

        goto out_clear;

    77

    78

    return 0;

    79

    80

out_clear:

    81

    ec_pdo_clear(pdo);

    82

out_return:

    83

    return -1;

    84

}

    85

    86

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

    87

    88

/** Pdo destructor.

    89

 */

    90

void ec_pdo_clear(ec_pdo_t *pdo /**< EtherCAT Pdo. */)

    91

{

    92

    if (pdo->name)

    93

        kfree(pdo->name);

    94

    95

    ec_pdo_clear_entries(pdo);

    96

}

    97

    98

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

    99

   100

/** Clear Pdo entry list.

   101

 */

   102

void ec_pdo_clear_entries(ec_pdo_t *pdo /**< EtherCAT Pdo. */)

   103

{

   104

    ec_pdo_entry_t *entry, *next;

   105

   106

    // free all Pdo entries

   107

    list_for_each_entry_safe(entry, next, &pdo->entries, list) {

   108

        list_del(&entry->list);

   109

        ec_pdo_entry_clear(entry);

   110

        kfree(entry);

   111

    }

   112

}

   113

   114

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

   115

   116

/** Set Pdo name.

   117

 */

   118

int ec_pdo_set_name(

   119

        ec_pdo_t *pdo, /**< Pdo. */

   120

        const char *name /**< New name. */

   121

        )

   122

{

   123

    unsigned int len;

   124

   125

    if (pdo->name)

   126

        kfree(pdo->name);

   127

   128

    if (name && (len = strlen(name))) {

   129

        if (!(pdo->name = (char *) kmalloc(len + 1, GFP_KERNEL))) {

   130

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

   131

            return -1;

   132

        }

   133

        memcpy(pdo->name, name, len + 1);

   134

    } else {

   135

        pdo->name = NULL;

   136

    }

   137

   138

    return 0;

   139

}

   140

   141

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

   142

   143

/** Add a new Pdo entry to the configuration.

   144

 */

   145

ec_pdo_entry_t *ec_pdo_add_entry(

   146

        ec_pdo_t *pdo,

   147

        uint16_t index,

   148

        uint8_t subindex,

   149

        uint8_t bit_length

   150

        )

   151

{

   152

    ec_pdo_entry_t *entry;

   153

   154

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

   155

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

   156

        return NULL;

   157

    }

   158

   159

    ec_pdo_entry_init(entry);

   160

    entry->index = index;

   161

    entry->subindex = subindex;

   162

    entry->bit_length = bit_length;

   163

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

   164

    return entry;

   165

}

   166

   167

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

   168

   169

/** Copy Pdo entries from another Pdo.

   170

 */

   171

int ec_pdo_copy_entries(ec_pdo_t *pdo, const ec_pdo_t *other)

   172

{

   173

    ec_pdo_entry_t *entry, *other_entry;

   174

   175

    ec_pdo_clear_entries(pdo);

   176

   177

    list_for_each_entry(other_entry, &other->entries, list) {

   178

        if (!(entry = (ec_pdo_entry_t *)

   179

                    kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) {

   180

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

   181

            return -1;

   182

        }

   183

   184

        if (ec_pdo_entry_init_copy(entry, other_entry)) {

   185

            kfree(entry);

   186

            return -1;

   187

        }

   188

   189

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

   190

    }

   191

   192

    return 0;

   193

}

   194

   195

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

   196

   197

/** Compares the entries of two Pdos.

   198

 *

   199

 * \retval 1 The entries of the given Pdos are equal.

   200

 * \retval 0 The entries of the given Pdos differ.

   201

 */

   202

int ec_pdo_equal_entries(

   203

        const ec_pdo_t *pdo1, /**< First Pdo. */

   204

        const ec_pdo_t *pdo2 /**< Second Pdo. */

   205

        )

   206

{

   207

    const struct list_head *head1, *head2, *item1, *item2;

   208

    const ec_pdo_entry_t *entry1, *entry2;

   209

   210

    head1 = item1 = &pdo1->entries;

   211

    head2 = item2 = &pdo2->entries;

   212

   213

    while (1) {

   214

        item1 = item1->next;

   215

        item2 = item2->next;

   216

   217

        if ((item1 == head1) ^ (item2 == head2)) // unequal lengths

   218

            return 0;

   219

        if (item1 == head1) // both finished

   220

            break;

   221

   222

        entry1 = list_entry(item1, ec_pdo_entry_t, list);

   223

        entry2 = list_entry(item2, ec_pdo_entry_t, list);

   224

        if (!ec_pdo_entry_equal(entry1, entry2))

   225

            return 0;

   226

    }

   227

   228

    return 1;

   229

}

   230

   231

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

   232

   233

/**

   234

 */

   235

unsigned int ec_pdo_entry_count(

   236

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

   237

        )

   238

{

   239

    const ec_pdo_entry_t *entry;

   240

    unsigned int num = 0;

   241

   242

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

   243

        num++;

   244

    }

   245

   246

    return num;

   247

}

   248

   249

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

   250

   251

/** Finds a Pdo entry via its position in the list.

   252

 *

   253

 * Const version.

   254

 */

   255

const ec_pdo_entry_t *ec_pdo_find_entry_by_pos_const(

   256

        const ec_pdo_t *pdo, /**< Pdo. */

   257

        unsigned int pos /**< Position in the list. */

   258

        )

   259

{

   260

    const ec_pdo_entry_t *entry;

   261

   262

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

   263

        if (pos--)

   264

            continue;

   265

        return entry;

   266

    }

   267

   268

    return NULL;

   269

}

   270

   271

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