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

#include <linux/err.h>

    43

    44

#include "pdo.h"

    45

    46

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

    47

    48

/** Pdo constructor.

    49

 */

    50

void ec_pdo_init(

    51

        ec_pdo_t *pdo /**< EtherCAT Pdo */

    52

        )

    53

{

    54

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

    55

    pdo->name = NULL;

    56

    INIT_LIST_HEAD(&pdo->entries);

    57

}

    58

    59

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

    60

    61

/** Pdo copy constructor.

    62

 *

    63

 * \retval  0 Success.

    64

 * \retval <0 Error code.

    65

 */

    66

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

    67

{

    68

    int ret = 0;

    69

    70

    pdo->index = other_pdo->index;

    71

    pdo->sync_index = other_pdo->sync_index;

    72

    pdo->name = NULL;

    73

    INIT_LIST_HEAD(&pdo->entries);

    74

    75

    ret = ec_pdo_set_name(pdo, other_pdo->name);

    76

    if (ret < 0)

    77

        goto out_return;

    78

    79

    ret = ec_pdo_copy_entries(pdo, other_pdo);

    80

    if (ret < 0)

    81

        goto out_clear;

    82

    83

    return 0;

    84

    85

out_clear:

    86

    ec_pdo_clear(pdo);

    87

out_return:

    88

    return ret;

    89

}

    90

    91

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

    92

    93

/** Pdo destructor.

    94

 */

    95

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

    96

{

    97

    if (pdo->name)

    98

        kfree(pdo->name);

    99

   100

    ec_pdo_clear_entries(pdo);

   101

}

   102

   103

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

   104

   105

/** Clear Pdo entry list.

   106

 */

   107

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

   108

{

   109

    ec_pdo_entry_t *entry, *next;

   110

   111

    // free all Pdo entries

   112

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

   113

        list_del(&entry->list);

   114

        ec_pdo_entry_clear(entry);

   115

        kfree(entry);

   116

    }

   117

}

   118

   119

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

   120

   121

/** Set Pdo name.

   122

 *

   123

 * \retval  0 Success.

   124

 * \retval <0 Error code.

   125

 */

   126

int ec_pdo_set_name(

   127

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

   128

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

   129

        )

   130

{

   131

    unsigned int len;

   132

   133

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

   134

        return 0;

   135

   136

    if (pdo->name)

   137

        kfree(pdo->name);

   138

   139

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

   140

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

   141

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

   142

            return -ENOMEM;

   143

        }

   144

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

   145

    } else {

   146

        pdo->name = NULL;

   147

    }

   148

   149

    return 0;

   150

}

   151

   152

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

   153

   154

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

   155

 *

   156

 * \retval Pointer to the added entry, otherwise a ERR_PTR() code.

   157

 */

   158

ec_pdo_entry_t *ec_pdo_add_entry(

   159

        ec_pdo_t *pdo,

   160

        uint16_t index,

   161

        uint8_t subindex,

   162

        uint8_t bit_length

   163

        )

   164

{

   165

    ec_pdo_entry_t *entry;

   166

   167

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

   168

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

   169

        return ERR_PTR(-ENOMEM);

   170

    }

   171

   172

    ec_pdo_entry_init(entry);

   173

    entry->index = index;

   174

    entry->subindex = subindex;

   175

    entry->bit_length = bit_length;

   176

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

   177

    return entry;

   178

}

   179

   180

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

   181

   182

/** Copy Pdo entries from another Pdo.

   183

 *

   184

 * \retval  0 Success.

   185

 * \retval <0 Error code.

   186

 */

   187

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

   188

{

   189

    ec_pdo_entry_t *entry, *other_entry;

   190

    int ret;

   191

   192

    ec_pdo_clear_entries(pdo);

   193

   194

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

   195

        if (!(entry = (ec_pdo_entry_t *)

   196

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

   197

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

   198

            return -ENOMEM;

   199

        }

   200

   201

        ret = ec_pdo_entry_init_copy(entry, other_entry);

   202

        if (ret < 0) {

   203

            kfree(entry);

   204

            return ret;

   205

        }

   206

   207

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

   208

    }

   209

   210

    return 0;

   211

}

   212

   213

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

   214

   215

/** Compares the entries of two Pdos.

   216

 *

   217

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

   218

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

   219

 */

   220

int ec_pdo_equal_entries(

   221

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

   222

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

   223

        )

   224

{

   225

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

   226

    const ec_pdo_entry_t *entry1, *entry2;

   227

   228

    head1 = item1 = &pdo1->entries;

   229

    head2 = item2 = &pdo2->entries;

   230

   231

    while (1) {

   232

        item1 = item1->next;

   233

        item2 = item2->next;

   234

   235

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

   236

            return 0;

   237

        if (item1 == head1) // both finished

   238

            break;

   239

   240

        entry1 = list_entry(item1, ec_pdo_entry_t, list);

   241

        entry2 = list_entry(item2, ec_pdo_entry_t, list);

   242

        if (!ec_pdo_entry_equal(entry1, entry2))

   243

            return 0;

   244

    }

   245

   246

    return 1;

   247

}

   248

   249

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

   250

   251

/** Get the number of Pdo entries.

   252

 *

   253

 * \return Number of Pdo entries.

   254

 */

   255

unsigned int ec_pdo_entry_count(

   256

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

   257

        )

   258

{

   259

    const ec_pdo_entry_t *entry;

   260

    unsigned int num = 0;

   261

   262

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

   263

        num++;

   264

    }

   265

   266

    return num;

   267

}

   268

   269

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

   270

   271

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

   272

 *

   273

 * Const version.

   274

 */

   275

const ec_pdo_entry_t *ec_pdo_find_entry_by_pos_const(

   276

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

   277

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

   278

        )

   279

{

   280

    const ec_pdo_entry_t *entry;

   281

   282

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

   283

        if (pos--)

   284

            continue;

   285

        return entry;

   286

    }

   287

   288

    return NULL;

   289

}

   290

   291

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