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

 *  ---

    23

 *

    24

 *  The license mentioned above concerns the source code only. Using the

    25

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

    26

 *  industrial property and similar rights of Beckhoff Automation GmbH.

    27

 *

    28

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

    29

    30

/**

    31

   \file

    32

   EtherCAT process data object methods.

    33

*/

    34

    35

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

    36

    37

#include <linux/slab.h>

    38

#include <linux/err.h>

    39

    40

#include "pdo.h"

    41

    42

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

    43

    44

/** PDO constructor.

    45

 */

    46

void ec_pdo_init(

    47

        ec_pdo_t *pdo /**< EtherCAT PDO */

    48

        )

    49

{

    50

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

    51

    pdo->name = NULL;

    52

    INIT_LIST_HEAD(&pdo->entries);

    53

}

    54

    55

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

    56

    57

/** PDO copy constructor.

    58

 *

    59

 * \retval  0 Success.

    60

 * \retval <0 Error code.

    61

 */

    62

int ec_pdo_init_copy(

    63

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

    64

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

    65

        )

    66

{

    67

    int ret = 0;

    68

    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

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

    75

    if (ret < 0)

    76

        goto out_return;

    77

    78

    ret = ec_pdo_copy_entries(pdo, other_pdo);

    79

    if (ret < 0)

    80

        goto out_clear;

    81

    82

    return 0;

    83

    84

out_clear:

    85

    ec_pdo_clear(pdo);

    86

out_return:

    87

    return ret;

    88

}

    89

    90

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

    91

    92

/** PDO destructor.

    93

 */

    94

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

    95

{

    96

    if (pdo->name)

    97

        kfree(pdo->name);

    98

    99

    ec_pdo_clear_entries(pdo);

   100

}

   101

   102

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

   103

   104

/** Clear PDO entry list.

   105

 */

   106

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

   107

{

   108

    ec_pdo_entry_t *entry, *next;

   109

   110

    // free all PDO entries

   111

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

   112

        list_del(&entry->list);

   113

        ec_pdo_entry_clear(entry);

   114

        kfree(entry);

   115

    }

   116

}

   117

   118

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

   119

   120

/** Set PDO name.

   121

 *

   122

 * \retval  0 Success.

   123

 * \retval <0 Error code.

   124

 */

   125

int ec_pdo_set_name(

   126

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

   127

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

   128

        )

   129

{

   130

    unsigned int len;

   131

   132

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

   133

        return 0;

   134

   135

    if (pdo->name)

   136

        kfree(pdo->name);

   137

   138

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

   139

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

   140

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

   141

            return -ENOMEM;

   142

        }

   143

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

   144

    } else {

   145

        pdo->name = NULL;

   146

    }

   147

   148

    return 0;

   149

}

   150

   151

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

   152

   153

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

   154

 *

   155

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

   156

 */

   157

ec_pdo_entry_t *ec_pdo_add_entry(

   158

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

   159

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

   160

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

   161

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

   162

        )

   163

{

   164

    ec_pdo_entry_t *entry;

   165

   166

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

   167

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

   168

        return ERR_PTR(-ENOMEM);

   169

    }

   170

   171

    ec_pdo_entry_init(entry);

   172

    entry->index = index;

   173

    entry->subindex = subindex;

   174

    entry->bit_length = bit_length;

   175

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

   176

    return entry;

   177

}

   178

   179

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

   180

   181

/** Copy PDO entries from another PDO.

   182

 *

   183

 * \retval  0 Success.

   184

 * \retval <0 Error code.

   185

 */

   186

int ec_pdo_copy_entries(

   187

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

   188

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

   189

        )

   190

{

   191

    ec_pdo_entry_t *entry, *other_entry;

   192

    int ret;

   193

   194

    ec_pdo_clear_entries(pdo);

   195

   196

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

   197

        if (!(entry = (ec_pdo_entry_t *)

   198

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

   199

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

   200

            return -ENOMEM;

   201

        }

   202

   203

        ret = ec_pdo_entry_init_copy(entry, other_entry);

   204

        if (ret < 0) {

   205

            kfree(entry);

   206

            return ret;

   207

        }

   208

   209

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

   210

    }

   211

   212

    return 0;

   213

}

   214

   215

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

   216

   217

/** Compares the entries of two PDOs.

   218

 *

   219

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

   220

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

   221

 */

   222

int ec_pdo_equal_entries(

   223

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

   224

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

   225

        )

   226

{

   227

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

   228

    const ec_pdo_entry_t *entry1, *entry2;

   229

   230

    head1 = item1 = &pdo1->entries;

   231

    head2 = item2 = &pdo2->entries;

   232

   233

    while (1) {

   234

        item1 = item1->next;

   235

        item2 = item2->next;

   236

   237

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

   238

            return 0;

   239

        if (item1 == head1) // both finished

   240

            break;

   241

   242

        entry1 = list_entry(item1, ec_pdo_entry_t, list);

   243

        entry2 = list_entry(item2, ec_pdo_entry_t, list);

   244

        if (!ec_pdo_entry_equal(entry1, entry2))

   245

            return 0;

   246

    }

   247

   248

    return 1;

   249

}

   250

   251

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

   252

   253

/** Get the number of PDO entries.

   254

 *

   255

 * \return Number of PDO entries.

   256

 */

   257

unsigned int ec_pdo_entry_count(

   258

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

   259

        )

   260

{

   261

    const ec_pdo_entry_t *entry;

   262

    unsigned int num = 0;

   263

   264

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

   265

        num++;

   266

    }

   267

   268

    return num;

   269

}

   270

   271

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

   272

   273

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

   274

 *

   275

 * Const version.

   276

 *

   277

 * \return Search result, or NULL.

   278

 */

   279

const ec_pdo_entry_t *ec_pdo_find_entry_by_pos_const(

   280

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

   281

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

   282

        )

   283

{

   284

    const ec_pdo_entry_t *entry;

   285

   286

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

   287

        if (pos--)

   288

            continue;

   289

        return entry;

   290

    }

   291

   292

    return NULL;

   293

}

   294

   295

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

   296

   297

/** Outputs the PDOs in the list.

   298

 */

   299

void ec_pdo_print_entries(

   300

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

   301

        )

   302

{

   303

    const ec_pdo_entry_t *entry;

   304

   305

    if (list_empty(&pdo->entries)) {

   306

        printk("(none)");

   307

    } else {

   308

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

   309

            printk("0x%04X:%02X/%u",

   310

                    entry->index, entry->subindex, entry->bit_length);

   311

            if (entry->list.next != &pdo->entries)

   312

                printk(" ");

   313

        }

   314

    }

   315

}

   316

   317

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