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 PDO list methods.

    30

*/

    31

    32

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

    33

    34

#include <linux/module.h>

    35

    36

#include "globals.h"

    37

#include "pdo.h"

    38

#include "slave_config.h"

    39

#include "master.h"

    40

    41

#include "pdo_list.h"

    42

    43

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

    44

    45

/** PDO list constructor.

    46

 */

    47

void ec_pdo_list_init(

    48

        ec_pdo_list_t *pl /**< PDO list. */

    49

        )

    50

{

    51

    INIT_LIST_HEAD(&pl->list);

    52

}

    53

    54

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

    55

    56

/** PDO list destructor.

    57

 */

    58

void ec_pdo_list_clear(ec_pdo_list_t *pl /**< PDO list. */)

    59

{

    60

    ec_pdo_list_clear_pdos(pl);

    61

}

    62

    63

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

    64

    65

/** Clears the list of mapped PDOs.

    66

 */

    67

void ec_pdo_list_clear_pdos(ec_pdo_list_t *pl /**< PDO list. */)

    68

{

    69

    ec_pdo_t *pdo, *next;

    70

    71

    list_for_each_entry_safe(pdo, next, &pl->list, list) {

    72

        list_del_init(&pdo->list);

    73

        ec_pdo_clear(pdo);

    74

        kfree(pdo);

    75

    }

    76

}

    77

    78

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

    79

    80

/** Calculates the total size of the mapped PDO entries.

    81

 *

    82

 * \retval Data size in byte.

    83

 */

    84

uint16_t ec_pdo_list_total_size(

    85

        const ec_pdo_list_t *pl /**< PDO list. */

    86

        )

    87

{

    88

    unsigned int bit_size;

    89

    const ec_pdo_t *pdo;

    90

    const ec_pdo_entry_t *pdo_entry;

    91

    uint16_t byte_size;

    92

    93

    bit_size = 0;

    94

    list_for_each_entry(pdo, &pl->list, list) {

    95

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

    96

            bit_size += pdo_entry->bit_length;

    97

        }

    98

    }

    99

   100

    if (bit_size % 8) // round up to full bytes

   101

        byte_size = bit_size / 8 + 1;

   102

    else

   103

        byte_size = bit_size / 8;

   104

   105

    return byte_size;

   106

}

   107

   108

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

   109

   110

/** Add a new PDO to the list.

   111

 *

   112

 * \retval >0 Pointer to new PDO.

   113

 * \retval NULL No memory.

   114

 */

   115

ec_pdo_t *ec_pdo_list_add_pdo(

   116

        ec_pdo_list_t *pl, /**< PDO list. */

   117

        uint16_t index /**< PDO index. */

   118

        )

   119

{

   120

    ec_pdo_t *pdo;

   121

   122

    if (!(pdo = (ec_pdo_t *) kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {

   123

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

   124

        return NULL;

   125

    }

   126

   127

    ec_pdo_init(pdo);

   128

    pdo->index = index;

   129

    list_add_tail(&pdo->list, &pl->list);

   130

    return pdo;

   131

}

   132

   133

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

   134

   135

/** Add the copy of an existing PDO to the list.

   136

 *

   137

 * \return 0 on success, else < 0

   138

 */

   139

int ec_pdo_list_add_pdo_copy(

   140

        ec_pdo_list_t *pl, /**< PDO list. */

   141

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

   142

        )

   143

{

   144

    ec_pdo_t *mapped_pdo;

   145

   146

    // PDO already mapped?

   147

    list_for_each_entry(mapped_pdo, &pl->list, list) {

   148

        if (mapped_pdo->index != pdo->index) continue;

   149

        EC_ERR("PDO 0x%04X is already mapped!\n", pdo->index);

   150

        return -1;

   151

    }

   152

   153

    if (!(mapped_pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {

   154

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

   155

        return -1;

   156

    }

   157

   158

    if (ec_pdo_init_copy(mapped_pdo, pdo)) {

   159

        kfree(mapped_pdo);

   160

        return -1;

   161

    }

   162

   163

    list_add_tail(&mapped_pdo->list, &pl->list);

   164

    return 0;

   165

}

   166

   167

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

   168

   169

/** Makes a deep copy of another PDO list.

   170

 *

   171

 * \return 0 on success, else < 0

   172

 */

   173

int ec_pdo_list_copy(

   174

        ec_pdo_list_t *pl, /**< PDO list. */

   175

        const ec_pdo_list_t *other /**< PDO list to copy from. */

   176

        )

   177

{

   178

    ec_pdo_t *other_pdo;

   179

   180

    ec_pdo_list_clear_pdos(pl);

   181

   182

    // PDO already mapped?

   183

    list_for_each_entry(other_pdo, &other->list, list) {

   184

        if (ec_pdo_list_add_pdo_copy(pl, other_pdo))

   185

            return -1;

   186

    }

   187

   188

    return 0;

   189

}

   190

   191

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

   192

   193

/** Compares two PDO lists.

   194

 *

   195

 * Only the list is compared, not the PDO entries (i. e. the PDO

   196

 * mapping).

   197

 *

   198

 * \retval 1 The given PDO lists are equal.

   199

 * \retval 0 The given PDO lists differ.

   200

 */

   201

int ec_pdo_list_equal(

   202

        const ec_pdo_list_t *pl1, /**< First list. */

   203

        const ec_pdo_list_t *pl2 /**< Second list. */

   204

        )

   205

{

   206

    const struct list_head *h1, *h2, *l1, *l2;

   207

    const ec_pdo_t *p1, *p2;

   208

   209

    h1 = l1 = &pl1->list;

   210

    h2 = l2 = &pl2->list;

   211

   212

    while (1) {

   213

        l1 = l1->next;

   214

        l2 = l2->next;

   215

   216

        if ((l1 == h1) ^ (l2 == h2)) // unequal lengths

   217

            return 0;

   218

        if (l1 == h1) // both finished

   219

            break;

   220

   221

        p1 = list_entry(l1, ec_pdo_t, list);

   222

        p2 = list_entry(l2, ec_pdo_t, list);

   223

   224

        if (p1->index != p2->index)

   225

            return 0;

   226

    }

   227

   228

    return 1;

   229

}

   230

   231

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

   232

   233

/** Finds a PDO with the given index.

   234

 */

   235

ec_pdo_t *ec_pdo_list_find_pdo(

   236

        const ec_pdo_list_t *pl, /**< PDO list. */

   237

        uint16_t index /**< PDO index. */

   238

        )

   239

{

   240

    ec_pdo_t *pdo;

   241

   242

    list_for_each_entry(pdo, &pl->list, list) {

   243

        if (pdo->index != index)

   244

            continue;

   245

        return pdo;

   246

    }

   247

   248

    return NULL;

   249

}

   250

   251

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

   252

   253

/** Finds a PDO with the given index and returns a const pointer.

   254

 */

   255

const ec_pdo_t *ec_pdo_list_find_pdo_const(

   256

        const ec_pdo_list_t *pl, /**< PDO list. */

   257

        uint16_t index /**< PDO index. */

   258

        )

   259

{

   260

    const ec_pdo_t *pdo;

   261

   262

    list_for_each_entry(pdo, &pl->list, list) {

   263

        if (pdo->index != index)

   264

            continue;

   265

        return pdo;

   266

    }

   267

   268

    return NULL;

   269

}

   270

   271

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

   272

   273

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

   274

 *

   275

 * Const version.

   276

 */

   277

const ec_pdo_t *ec_pdo_list_find_pdo_by_pos_const(

   278

        const ec_pdo_list_t *pl, /**< PDO list. */

   279

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

   280

        )

   281

{

   282

    const ec_pdo_t *pdo;

   283

   284

    list_for_each_entry(pdo, &pl->list, list) {

   285

        if (pos--)

   286

            continue;

   287

        return pdo;

   288

    }

   289

   290

    return NULL;

   291

}

   292

   293

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

   294

   295

/** Get the number of PDOs in the list.

   296

 *

   297

 * \return Number of PDOs.

   298

 */

   299

unsigned int ec_pdo_list_count(

   300

        const ec_pdo_list_t *pl /**< PDO list. */

   301

        )

   302

{

   303

    const ec_pdo_t *pdo;

   304

    unsigned int num = 0;

   305

   306

    list_for_each_entry(pdo, &pl->list, list) {

   307

        num++;

   308

    }

   309

   310

    return num;

   311

}

   312

   313

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

   314

   315

/** Outputs the PDOs in the list.

   316

 */

   317

void ec_pdo_list_print(

   318

        const ec_pdo_list_t *pl /**< PDO list. */

   319

        )

   320

{

   321

    const ec_pdo_t *pdo;

   322

   323

    list_for_each_entry(pdo, &pl->list, list) {

   324

        printk("0x%04X", pdo->index);

   325

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

   326

            printk(" ");

   327

    }

   328

}

   329

   330

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