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->dir = other_pdo->dir;

    65

    pdo->index = other_pdo->index;

    66

    pdo->sync_index = other_pdo->sync_index;

    67

    pdo->name = NULL;

    68

    INIT_LIST_HEAD(&pdo->entries);

    69

    70

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

    71

        goto out_return;

    72

    73

    if (ec_pdo_copy_entries(pdo, other_pdo))

    74

        goto out_clear;

    75

    76

    return 0;

    77

    78

out_clear:

    79

    ec_pdo_clear(pdo);

    80

out_return:

    81

    return -1;

    82

}

    83

    84

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

    85

    86

/** Pdo destructor.

    87

 */

    88

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

    89

{

    90

    if (pdo->name)

    91

        kfree(pdo->name);

    92

    93

    ec_pdo_clear_entries(pdo);

    94

}

    95

    96

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

    97

    98

/** Clear Pdo entry list.

    99

 */

   100

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

   101

{

   102

    ec_pdo_entry_t *entry, *next;

   103

   104

    // free all Pdo entries

   105

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

   106

        list_del(&entry->list);

   107

        ec_pdo_entry_clear(entry);

   108

        kfree(entry);

   109

    }

   110

}

   111

   112

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

   113

   114

/** Set Pdo name.

   115

 */

   116

int ec_pdo_set_name(

   117

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

   118

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

   119

        )

   120

{

   121

    unsigned int len;

   122

   123

    if (pdo->name)

   124

        kfree(pdo->name);

   125

   126

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

   127

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

   128

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

   129

            return -1;

   130

        }

   131

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

   132

    } else {

   133

        pdo->name = NULL;

   134

    }

   135

   136

    return 0;

   137

}

   138

   139

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

   140

   141

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

   142

 */

   143

ec_pdo_entry_t *ec_pdo_add_entry(

   144

        ec_pdo_t *pdo,

   145

        uint16_t index,

   146

        uint8_t subindex,

   147

        uint8_t bit_length

   148

        )

   149

{

   150

    ec_pdo_entry_t *entry;

   151

   152

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

   153

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

   154

        return NULL;

   155

    }

   156

   157

    ec_pdo_entry_init(entry);

   158

    entry->index = index;

   159

    entry->subindex = subindex;

   160

    entry->bit_length = bit_length;

   161

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

   162

    return entry;

   163

}

   164

   165

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

   166

   167

/** Copy Pdo entries from another Pdo.

   168

 */

   169

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

   170

{

   171

    ec_pdo_entry_t *entry, *other_entry;

   172

   173

    ec_pdo_clear_entries(pdo);

   174

   175

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

   176

        if (!(entry = (ec_pdo_entry_t *)

   177

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

   178

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

   179

            return -1;

   180

        }

   181

   182

        if (ec_pdo_entry_init_copy(entry, other_entry)) {

   183

            kfree(entry);

   184

            return -1;

   185

        }

   186

   187

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

   188

    }

   189

   190

    return 0;

   191

}

   192

   193

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

   194

   195

/** Compares the entries of two Pdos.

   196

 *

   197

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

   198

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

   199

 */

   200

int ec_pdo_equal_entries(

   201

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

   202

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

   203

        )

   204

{

   205

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

   206

    const ec_pdo_entry_t *entry1, *entry2;

   207

   208

    head1 = item1 = &pdo1->entries;

   209

    head2 = item2 = &pdo2->entries;

   210

   211

    while (1) {

   212

        item1 = item1->next;

   213

        item2 = item2->next;

   214

   215

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

   216

            return 0;

   217

        if (item1 == head1) // both finished

   218

            break;

   219

   220

        entry1 = list_entry(item1, ec_pdo_entry_t, list);

   221

        entry2 = list_entry(item2, ec_pdo_entry_t, list);

   222

        if (!ec_pdo_entry_equal(entry1, entry2))

   223

            return 0;

   224

    }

   225

   226

    return 1;

   227

}

   228

   229

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

   230

   231

/**

   232

 */

   233

unsigned int ec_pdo_entry_count(

   234

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

   235

        )

   236

{

   237

    const ec_pdo_entry_t *entry;

   238

    unsigned int num = 0;

   239

   240

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

   241

        num++;

   242

    }

   243

   244

    return num;

   245

}

   246

   247

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

   248

   249

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

   250

 *

   251

 * Const version.

   252

 */

   253

const ec_pdo_entry_t *ec_pdo_find_entry_by_pos_const(

   254

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

   255

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

   256

        )

   257

{

   258

    const ec_pdo_entry_t *entry;

   259

   260

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

   261

        if (pos--)

   262

            continue;

   263

        return entry;

   264

    }

   265

   266

    return NULL;

   267

}

   268

   269

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