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

}

    57

    58

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

    59

    60

/** Pdo copy constructor.

    61

 */

    62

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

    63

{

    64

    pdo->index = other_pdo->index;

    65

    pdo->sync_index = other_pdo->sync_index;

    66

    pdo->name = NULL;

    67

    INIT_LIST_HEAD(&pdo->entries);

    68

    69

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

    70

        goto out_return;

    71

    72

    if (ec_pdo_copy_entries(pdo, other_pdo))

    73

        goto out_clear;

    74

    75

    return 0;

    76

    77

out_clear:

    78

    ec_pdo_clear(pdo);

    79

out_return:

    80

    return -1;

    81

}

    82

    83

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

    84

    85

/** Pdo destructor.

    86

 */

    87

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

    88

{

    89

    if (pdo->name)

    90

        kfree(pdo->name);

    91

    92

    ec_pdo_clear_entries(pdo);

    93

}

    94

    95

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

    96

    97

/** Clear Pdo entry list.

    98

 */

    99

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

   100

{

   101

    ec_pdo_entry_t *entry, *next;

   102

   103

    // free all Pdo entries

   104

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

   105

        list_del(&entry->list);

   106

        ec_pdo_entry_clear(entry);

   107

        kfree(entry);

   108

    }

   109

}

   110

   111

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

   112

   113

/** Set Pdo name.

   114

 */

   115

int ec_pdo_set_name(

   116

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

   117

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

   118

        )

   119

{

   120

    unsigned int len;

   121

   122

    if (pdo->name && name && !strcmp(pdo->name, name))

   123

        return 0;

   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

/** Get the number of Pdo entries.

   234

 *

   235

 * \return Number of Pdo entries.

   236

 */

   237

unsigned int ec_pdo_entry_count(

   238

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

   239

        )

   240

{

   241

    const ec_pdo_entry_t *entry;

   242

    unsigned int num = 0;

   243

   244

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

   245

        num++;

   246

    }

   247

   248

    return num;

   249

}

   250

   251

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

   252

   253

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

   254

 *

   255

 * Const version.

   256

 */

   257

const ec_pdo_entry_t *ec_pdo_find_entry_by_pos_const(

   258

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

   259

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

   260

        )

   261

{

   262

    const ec_pdo_entry_t *entry;

   263

   264

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

   265

        if (pos--)

   266

            continue;

   267

        return entry;

   268

    }

   269

   270

    return NULL;

   271

}

   272

   273

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