1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006-2008  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 and/or

    10

 *  modify it under the terms of the GNU General Public License version 2, as

    11

 *  published by the Free Software Foundation.

    12

 *

    13

 *  The IgH EtherCAT Master is distributed in the hope that it will be useful,

    14

 *  but WITHOUT ANY WARRANTY; without even the implied warranty of

    15

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General

    16

 *  Public License for more details.

    17

 *

    18

 *  You should have received a copy of the GNU General Public License along

    19

 *  with the IgH EtherCAT Master; if not, write to the Free Software

    20

 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    21

 *

    22

 *  Using the EtherCAT technology and brand is permitted in compliance with

    23

 *  the industrial property and similar rights of Beckhoff Automation GmbH.

    24

 *

    25

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

    26

    27

/**

    28

   \file

    29

   EtherCAT process data object methods.

    30

*/

    31

    32

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

    33

    34

#include <linux/slab.h>

    35

    36

#include "pdo.h"

    37

    38

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

    39

    40

/** PDO constructor.

    41

 */

    42

void ec_pdo_init(

    43

        ec_pdo_t *pdo /**< EtherCAT PDO */

    44

        )

    45

{

    46

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

    47

    pdo->name = NULL;

    48

    INIT_LIST_HEAD(&pdo->entries);

    49

}

    50

    51

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

    52

    53

/** PDO copy constructor.

    54

 */

    55

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

    56

{

    57

    pdo->index = other_pdo->index;

    58

    pdo->sync_index = other_pdo->sync_index;

    59

    pdo->name = NULL;

    60

    INIT_LIST_HEAD(&pdo->entries);

    61

    62

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

    63

        goto out_return;

    64

    65

    if (ec_pdo_copy_entries(pdo, other_pdo))

    66

        goto out_clear;

    67

    68

    return 0;

    69

    70

out_clear:

    71

    ec_pdo_clear(pdo);

    72

out_return:

    73

    return -1;

    74

}

    75

    76

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

    77

    78

/** PDO destructor.

    79

 */

    80

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

    81

{

    82

    if (pdo->name)

    83

        kfree(pdo->name);

    84

    85

    ec_pdo_clear_entries(pdo);

    86

}

    87

    88

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

    89

    90

/** Clear PDO entry list.

    91

 */

    92

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

    93

{

    94

    ec_pdo_entry_t *entry, *next;

    95

    96

    // free all PDO entries

    97

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

    98

        list_del(&entry->list);

    99

        ec_pdo_entry_clear(entry);

   100

        kfree(entry);

   101

    }

   102

}

   103

   104

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

   105

   106

/** Set PDO name.

   107

 */

   108

int ec_pdo_set_name(

   109

        ec_pdo_t *pdo, /**< PDO. */

   110

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

   111

        )

   112

{

   113

    unsigned int len;

   114

   115

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

   116

        return 0;

   117

   118

    if (pdo->name)

   119

        kfree(pdo->name);

   120

   121

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

   122

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

   123

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

   124

            return -1;

   125

        }

   126

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

   127

    } else {

   128

        pdo->name = NULL;

   129

    }

   130

   131

    return 0;

   132

}

   133

   134

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

   135

   136

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

   137

 */

   138

ec_pdo_entry_t *ec_pdo_add_entry(

   139

        ec_pdo_t *pdo,

   140

        uint16_t index,

   141

        uint8_t subindex,

   142

        uint8_t bit_length

   143

        )

   144

{

   145

    ec_pdo_entry_t *entry;

   146

   147

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

   148

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

   149

        return NULL;

   150

    }

   151

   152

    ec_pdo_entry_init(entry);

   153

    entry->index = index;

   154

    entry->subindex = subindex;

   155

    entry->bit_length = bit_length;

   156

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

   157

    return entry;

   158

}

   159

   160

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

   161

   162

/** Copy PDO entries from another PDO.

   163

 */

   164

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

   165

{

   166

    ec_pdo_entry_t *entry, *other_entry;

   167

   168

    ec_pdo_clear_entries(pdo);

   169

   170

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

   171

        if (!(entry = (ec_pdo_entry_t *)

   172

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

   173

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

   174

            return -1;

   175

        }

   176

   177

        if (ec_pdo_entry_init_copy(entry, other_entry)) {

   178

            kfree(entry);

   179

            return -1;

   180

        }

   181

   182

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

   183

    }

   184

   185

    return 0;

   186

}

   187

   188

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

   189

   190

/** Compares the entries of two PDOs.

   191

 *

   192

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

   193

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

   194

 */

   195

int ec_pdo_equal_entries(

   196

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

   197

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

   198

        )

   199

{

   200

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

   201

    const ec_pdo_entry_t *entry1, *entry2;

   202

   203

    head1 = item1 = &pdo1->entries;

   204

    head2 = item2 = &pdo2->entries;

   205

   206

    while (1) {

   207

        item1 = item1->next;

   208

        item2 = item2->next;

   209

   210

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

   211

            return 0;

   212

        if (item1 == head1) // both finished

   213

            break;

   214

   215

        entry1 = list_entry(item1, ec_pdo_entry_t, list);

   216

        entry2 = list_entry(item2, ec_pdo_entry_t, list);

   217

        if (!ec_pdo_entry_equal(entry1, entry2))

   218

            return 0;

   219

    }

   220

   221

    return 1;

   222

}

   223

   224

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

   225

   226

/** Get the number of PDO entries.

   227

 *

   228

 * \return Number of PDO entries.

   229

 */

   230

unsigned int ec_pdo_entry_count(

   231

        const ec_pdo_t *pdo /**< PDO. */

   232

        )

   233

{

   234

    const ec_pdo_entry_t *entry;

   235

    unsigned int num = 0;

   236

   237

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

   238

        num++;

   239

    }

   240

   241

    return num;

   242

}

   243

   244

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

   245

   246

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

   247

 *

   248

 * Const version.

   249

 */

   250

const ec_pdo_entry_t *ec_pdo_find_entry_by_pos_const(

   251

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

   252

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

   253

        )

   254

{

   255

    const ec_pdo_entry_t *entry;

   256

   257

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

   258

        if (pos--)

   259

            continue;

   260

        return entry;

   261

    }

   262

   263

    return NULL;

   264

}

   265

   266

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