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)

   123

        kfree(pdo->name);

   124

   125

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

   126

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

   127

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

   128

            return -1;

   129

        }

   130

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

   131

    } else {

   132

        pdo->name = NULL;

   133

    }

   134

   135

    return 0;

   136

}

   137

   138

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

   139

   140

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

   141

 */

   142

ec_pdo_entry_t *ec_pdo_add_entry(

   143

        ec_pdo_t *pdo,

   144

        uint16_t index,

   145

        uint8_t subindex,

   146

        uint8_t bit_length

   147

        )

   148

{

   149

    ec_pdo_entry_t *entry;

   150

   151

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

   152

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

   153

        return NULL;

   154

    }

   155

   156

    ec_pdo_entry_init(entry);

   157

    entry->index = index;

   158

    entry->subindex = subindex;

   159

    entry->bit_length = bit_length;

   160

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

   161

    return entry;

   162

}

   163

   164

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

   165

   166

/** Copy Pdo entries from another Pdo.

   167

 */

   168

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

   169

{

   170

    ec_pdo_entry_t *entry, *other_entry;

   171

   172

    ec_pdo_clear_entries(pdo);

   173

   174

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

   175

        if (!(entry = (ec_pdo_entry_t *)

   176

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

   177

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

   178

            return -1;

   179

        }

   180

   181

        if (ec_pdo_entry_init_copy(entry, other_entry)) {

   182

            kfree(entry);

   183

            return -1;

   184

        }

   185

   186

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

   187

    }

   188

   189

    return 0;

   190

}

   191

   192

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

   193

   194

/** Compares the entries of two Pdos.

   195

 *

   196

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

   197

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

   198

 */

   199

int ec_pdo_equal_entries(

   200

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

   201

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

   202

        )

   203

{

   204

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

   205

    const ec_pdo_entry_t *entry1, *entry2;

   206

   207

    head1 = item1 = &pdo1->entries;

   208

    head2 = item2 = &pdo2->entries;

   209

   210

    while (1) {

   211

        item1 = item1->next;

   212

        item2 = item2->next;

   213

   214

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

   215

            return 0;

   216

        if (item1 == head1) // both finished

   217

            break;

   218

   219

        entry1 = list_entry(item1, ec_pdo_entry_t, list);

   220

        entry2 = list_entry(item2, ec_pdo_entry_t, list);

   221

        if (!ec_pdo_entry_equal(entry1, entry2))

   222

            return 0;

   223

    }

   224

   225

    return 1;

   226

}

   227

   228

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

   229

   230

/** Get the number of Pdo entries.

   231

 *

   232

 * \return Number of Pdo entries.

   233

 */

   234

unsigned int ec_pdo_entry_count(

   235

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

   236

        )

   237

{

   238

    const ec_pdo_entry_t *entry;

   239

    unsigned int num = 0;

   240

   241

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

   242

        num++;

   243

    }

   244

   245

    return num;

   246

}

   247

   248

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

   249

   250

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

   251

 *

   252

 * Const version.

   253

 */

   254

const ec_pdo_entry_t *ec_pdo_find_entry_by_pos_const(

   255

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

   256

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

   257

        )

   258

{

   259

    const ec_pdo_entry_t *entry;

   260

   261

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

   262

        if (pos--)

   263

            continue;

   264

        return entry;

   265

    }

   266

   267

    return NULL;

   268

}

   269

   270

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