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

#include <linux/err.h>

    36

    37

#include "pdo.h"

    38

    39

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

    40

    41

/** PDO constructor.

    42

 */

    43

void ec_pdo_init(

    44

        ec_pdo_t *pdo /**< EtherCAT PDO */

    45

        )

    46

{

    47

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

    48

    pdo->name = NULL;

    49

    INIT_LIST_HEAD(&pdo->entries);

    50

}

    51

    52

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

    53

    54

/** PDO copy constructor.

    55

 *

    56

 * \retval  0 Success.

    57

 * \retval <0 Error code.

    58

 */

    59

int ec_pdo_init_copy(

    60

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

    61

        const ec_pdo_t *other_pdo /**< PDO to copy from. */

    62

        )

    63

{

    64

    int ret = 0;

    65

    66

    pdo->index = other_pdo->index;

    67

    pdo->sync_index = other_pdo->sync_index;

    68

    pdo->name = NULL;

    69

    INIT_LIST_HEAD(&pdo->entries);

    70

    71

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

    72

    if (ret < 0)

    73

        goto out_return;

    74

    75

    ret = ec_pdo_copy_entries(pdo, other_pdo);

    76

    if (ret < 0)

    77

        goto out_clear;

    78

    79

    return 0;

    80

    81

out_clear:

    82

    ec_pdo_clear(pdo);

    83

out_return:

    84

    return ret;

    85

}

    86

    87

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

    88

    89

/** PDO destructor.

    90

 */

    91

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

    92

{

    93

    if (pdo->name)

    94

        kfree(pdo->name);

    95

    96

    ec_pdo_clear_entries(pdo);

    97

}

    98

    99

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

   100

   101

/** Clear PDO entry list.

   102

 */

   103

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

   104

{

   105

    ec_pdo_entry_t *entry, *next;

   106

   107

    // free all PDO entries

   108

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

   109

        list_del(&entry->list);

   110

        ec_pdo_entry_clear(entry);

   111

        kfree(entry);

   112

    }

   113

}

   114

   115

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

   116

   117

/** Set PDO name.

   118

 *

   119

 * \retval  0 Success.

   120

 * \retval <0 Error code.

   121

 */

   122

int ec_pdo_set_name(

   123

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

   124

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

   125

        )

   126

{

   127

    unsigned int len;

   128

   129

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

   130

        return 0;

   131

   132

    if (pdo->name)

   133

        kfree(pdo->name);

   134

   135

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

   136

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

   137

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

   138

            return -ENOMEM;

   139

        }

   140

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

   141

    } else {

   142

        pdo->name = NULL;

   143

    }

   144

   145

    return 0;

   146

}

   147

   148

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

   149

   150

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

   151

 *

   152

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

   153

 */

   154

ec_pdo_entry_t *ec_pdo_add_entry(

   155

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

   156

        uint16_t index, /**< New entry's index. */

   157

        uint8_t subindex, /**< New entry's subindex. */

   158

        uint8_t bit_length /**< New entry's bit length. */

   159

        )

   160

{

   161

    ec_pdo_entry_t *entry;

   162

   163

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

   164

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

   165

        return ERR_PTR(-ENOMEM);

   166

    }

   167

   168

    ec_pdo_entry_init(entry);

   169

    entry->index = index;

   170

    entry->subindex = subindex;

   171

    entry->bit_length = bit_length;

   172

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

   173

    return entry;

   174

}

   175

   176

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

   177

   178

/** Copy PDO entries from another PDO.

   179

 *

   180

 * \retval  0 Success.

   181

 * \retval <0 Error code.

   182

 */

   183

int ec_pdo_copy_entries(

   184

        ec_pdo_t *pdo, /**< PDO whos entries shall be replaced. */

   185

        const ec_pdo_t *other /**< Pdo with entries to copy. */

   186

        )

   187

{

   188

    ec_pdo_entry_t *entry, *other_entry;

   189

    int ret;

   190

   191

    ec_pdo_clear_entries(pdo);

   192

   193

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

   194

        if (!(entry = (ec_pdo_entry_t *)

   195

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

   196

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

   197

            return -ENOMEM;

   198

        }

   199

   200

        ret = ec_pdo_entry_init_copy(entry, other_entry);

   201

        if (ret < 0) {

   202

            kfree(entry);

   203

            return ret;

   204

        }

   205

   206

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

   207

    }

   208

   209

    return 0;

   210

}

   211

   212

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

   213

   214

/** Compares the entries of two PDOs.

   215

 *

   216

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

   217

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

   218

 */

   219

int ec_pdo_equal_entries(

   220

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

   221

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

   222

        )

   223

{

   224

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

   225

    const ec_pdo_entry_t *entry1, *entry2;

   226

   227

    head1 = item1 = &pdo1->entries;

   228

    head2 = item2 = &pdo2->entries;

   229

   230

    while (1) {

   231

        item1 = item1->next;

   232

        item2 = item2->next;

   233

   234

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

   235

            return 0;

   236

        if (item1 == head1) // both finished

   237

            break;

   238

   239

        entry1 = list_entry(item1, ec_pdo_entry_t, list);

   240

        entry2 = list_entry(item2, ec_pdo_entry_t, list);

   241

        if (!ec_pdo_entry_equal(entry1, entry2))

   242

            return 0;

   243

    }

   244

   245

    return 1;

   246

}

   247

   248

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

   249

   250

/** Get the number of PDO entries.

   251

 *

   252

 * \return Number of PDO entries.

   253

 */

   254

unsigned int ec_pdo_entry_count(

   255

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

   256

        )

   257

{

   258

    const ec_pdo_entry_t *entry;

   259

    unsigned int num = 0;

   260

   261

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

   262

        num++;

   263

    }

   264

   265

    return num;

   266

}

   267

   268

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

   269

   270

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

   271

 *

   272

 * Const version.

   273

 */

   274

const ec_pdo_entry_t *ec_pdo_find_entry_by_pos_const(

   275

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

   276

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

   277

        )

   278

{

   279

    const ec_pdo_entry_t *entry;

   280

   281

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

   282

        if (pos--)

   283

            continue;

   284

        return entry;

   285

    }

   286

   287

    return NULL;

   288

}

   289

   290

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