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

void ec_pdo_clear_entries(ec_pdo_t *);

    48

    49

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

    50

    51

/** PDO constructor.

    52

 */

    53

void ec_pdo_init(

    54

        ec_pdo_t *pdo /**< EtherCAT PDO */

    55

        )

    56

{

    57

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

    58

    pdo->name = NULL;

    59

    INIT_LIST_HEAD(&pdo->entries);

    60

}

    61

    62

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

    63

    64

/** Pdo copy constructor.

    65

 */

    66

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

    67

{

    68

    pdo->dir = other_pdo->dir;

    69

    pdo->index = other_pdo->index;

    70

    pdo->sync_index = other_pdo->sync_index;

    71

    pdo->name = NULL;

    72

    INIT_LIST_HEAD(&pdo->entries);

    73

    74

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

    75

        goto out_return;

    76

    77

    if (ec_pdo_copy_entries(pdo, other_pdo))

    78

        goto out_clear;

    79

    80

    return 0;

    81

    82

out_clear:

    83

    ec_pdo_clear(pdo);

    84

out_return:

    85

    return -1;

    86

}

    87

    88

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

    89

    90

/** PDO destructor.

    91

 */

    92

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

    93

{

    94

    if (pdo->name)

    95

        kfree(pdo->name);

    96

    97

    ec_pdo_clear_entries(pdo);

    98

}

    99

   100

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

   101

   102

/** Clear Pdo entry list.

   103

 */

   104

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

   105

{

   106

    ec_pdo_entry_t *entry, *next;

   107

   108

    // free all PDO entries

   109

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

   110

        list_del(&entry->list);

   111

        ec_pdo_entry_clear(entry);

   112

        kfree(entry);

   113

    }

   114

}

   115

   116

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

   117

   118

/** Set Pdo name.

   119

 */

   120

int ec_pdo_set_name(

   121

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

   122

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

   123

        )

   124

{

   125

    unsigned int len;

   126

   127

    if (pdo->name)

   128

        kfree(pdo->name);

   129

   130

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

   131

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

   132

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

   133

            return -1;

   134

        }

   135

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

   136

    } else {

   137

        pdo->name = NULL;

   138

    }

   139

   140

    return 0;

   141

}

   142

   143

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

   144

   145

/** Copy Pdo entries from another Pdo.

   146

 */

   147

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

   148

{

   149

    ec_pdo_entry_t *entry, *other_entry;

   150

   151

    ec_pdo_clear_entries(pdo);

   152

   153

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

   154

        if (!(entry = (ec_pdo_entry_t *)

   155

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

   156

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

   157

            return -1;

   158

        }

   159

   160

        if (ec_pdo_entry_init_copy(entry, other_entry)) {

   161

            kfree(entry);

   162

            return -1;

   163

        }

   164

   165

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

   166

    }

   167

   168

    return 0;

   169

}

   170

   171

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

   172

   173

/** Compares the entries of two Pdos.

   174

 *

   175

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

   176

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

   177

 */

   178

int ec_pdo_equal_entries(

   179

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

   180

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

   181

        )

   182

{

   183

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

   184

    const ec_pdo_entry_t *entry1, *entry2;

   185

   186

    head1 = item1 = &pdo1->entries;

   187

    head2 = item2 = &pdo2->entries;

   188

   189

    while (1) {

   190

        item1 = item1->next;

   191

        item2 = item2->next;

   192

   193

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

   194

            return 0;

   195

        if (item1 == head1 && item2 == head2) // both finished

   196

            break;

   197

   198

        entry1 = list_entry(item1, ec_pdo_entry_t, list);

   199

        entry2 = list_entry(item2, ec_pdo_entry_t, list);

   200

        if (!ec_pdo_entry_equal(entry1, entry2))

   201

            return 0;

   202

    }

   203

   204

    return 1;

   205

}

   206

   207

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

   208

   209

/** Pdo entry constructor.

   210

 */

   211

void ec_pdo_entry_init(

   212

        ec_pdo_entry_t *entry /**< Pdo entry. */

   213

        )

   214

{

   215

    entry->name = NULL;

   216

}

   217

   218

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

   219

   220

/** Pdo entry copy constructor.

   221

 */

   222

int ec_pdo_entry_init_copy(

   223

        ec_pdo_entry_t *entry, /**< Pdo entry. */

   224

        const ec_pdo_entry_t *other /**< Pdo entry to copy from. */

   225

        )

   226

{

   227

    entry->index = other->index;

   228

    entry->subindex = other->subindex;

   229

    entry->name = NULL;

   230

    entry->bit_length = other->bit_length;

   231

   232

    if (ec_pdo_entry_set_name(entry, other->name))

   233

        return -1;

   234

   235

    return 0;

   236

}

   237

   238

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

   239

   240

/** Pdo entry destructor.

   241

 */

   242

void ec_pdo_entry_clear(ec_pdo_entry_t *entry /**< Pdo entry. */)

   243

{

   244

    if (entry->name)

   245

        kfree(entry->name);

   246

}

   247

   248

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

   249

   250

/** Set Pdo entry name.

   251

 */

   252

int ec_pdo_entry_set_name(

   253

        ec_pdo_entry_t *entry, /**< Pdo entry. */

   254

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

   255

        )

   256

{

   257

    unsigned int len;

   258

   259

    if (entry->name)

   260

        kfree(entry->name);

   261

   262

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

   263

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

   264

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

   265

            return -1;

   266

        }

   267

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

   268

    } else {

   269

        entry->name = NULL;

   270

    }

   271

   272

    return 0;

   273

}

   274

   275

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

   276

   277

/** Compares two Pdo entries.

   278

 *

   279

 * \retval 1 The entries are equal.

   280

 * \retval 0 The entries differ.

   281

 */

   282

int ec_pdo_entry_equal(

   283

        const ec_pdo_entry_t *entry1, /**< First Pdo entry. */

   284

        const ec_pdo_entry_t *entry2 /**< Second Pdo entry. */

   285

        )

   286

{

   287

    return entry1->index == entry2->index

   288

        && entry1->subindex == entry2->subindex

   289

        && entry1->bit_length == entry2->bit_length;

   290

}

   291

   292

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