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

 * \return Pointer to new PDO, otherwise an ERR_PTR() code.

   113

 */

   114

ec_pdo_t *ec_pdo_list_add_pdo(

   115

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

   116

        uint16_t index /**< PDO index. */

   117

        )

   118

{

   119

    ec_pdo_t *pdo;

   120

   121

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

   122

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

   123

        return ERR_PTR(-ENOMEM);

   124

    }

   125

   126

    ec_pdo_init(pdo);

   127

    pdo->index = index;

   128

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

   129

    return pdo;

   130

}

   131

   132

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

   133

   134

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

   135

 *

   136

 * \return 0 on success, else < 0

   137

 */

   138

int ec_pdo_list_add_pdo_copy(

   139

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

   140

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

   141

        )

   142

{

   143

    ec_pdo_t *mapped_pdo;

   144

    int ret;

   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 -EEXIST;

   151

    }

   152

   153

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

   154

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

   155

        return -ENOMEM;

   156

    }

   157

   158

    ret = ec_pdo_init_copy(mapped_pdo, pdo);

   159

    if (ret < 0) {

   160

        kfree(mapped_pdo);

   161

        return ret;

   162

    }

   163

   164

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

   165

    return 0;

   166

}

   167

   168

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

   169

   170

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

   171

 *

   172

 * \return 0 on success, else < 0

   173

 */

   174

int ec_pdo_list_copy(

   175

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

   176

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

   177

        )

   178

{

   179

    ec_pdo_t *other_pdo;

   180

    int ret;

   181

   182

    ec_pdo_list_clear_pdos(pl);

   183

   184

    // PDO already mapped?

   185

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

   186

        ret = ec_pdo_list_add_pdo_copy(pl, other_pdo);

   187

        if (ret)

   188

            return ret;

   189

    }

   190

   191

    return 0;

   192

}

   193

   194

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

   195

   196

/** Compares two PDO lists.

   197

 *

   198

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

   199

 * mapping).

   200

 *

   201

 * \retval 1 The given PDO lists are equal.

   202

 * \retval 0 The given PDO lists differ.

   203

 */

   204

int ec_pdo_list_equal(

   205

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

   206

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

   207

        )

   208

{

   209

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

   210

    const ec_pdo_t *p1, *p2;

   211

   212

    h1 = l1 = &pl1->list;

   213

    h2 = l2 = &pl2->list;

   214

   215

    while (1) {

   216

        l1 = l1->next;

   217

        l2 = l2->next;

   218

   219

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

   220

            return 0;

   221

        if (l1 == h1) // both finished

   222

            break;

   223

   224

        p1 = list_entry(l1, ec_pdo_t, list);

   225

        p2 = list_entry(l2, ec_pdo_t, list);

   226

   227

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

   228

            return 0;

   229

    }

   230

   231

    return 1;

   232

}

   233

   234

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

   235

   236

/** Finds a PDO with the given index.

   237

 */

   238

ec_pdo_t *ec_pdo_list_find_pdo(

   239

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

   240

        uint16_t index /**< PDO index. */

   241

        )

   242

{

   243

    ec_pdo_t *pdo;

   244

   245

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

   246

        if (pdo->index != index)

   247

            continue;

   248

        return pdo;

   249

    }

   250

   251

    return NULL;

   252

}

   253

   254

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

   255

   256

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

   257

 */

   258

const ec_pdo_t *ec_pdo_list_find_pdo_const(

   259

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

   260

        uint16_t index /**< PDO index. */

   261

        )

   262

{

   263

    const ec_pdo_t *pdo;

   264

   265

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

   266

        if (pdo->index != index)

   267

            continue;

   268

        return pdo;

   269

    }

   270

   271

    return NULL;

   272

}

   273

   274

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

   275

   276

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

   277

 *

   278

 * Const version.

   279

 */

   280

const ec_pdo_t *ec_pdo_list_find_pdo_by_pos_const(

   281

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

   282

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

   283

        )

   284

{

   285

    const ec_pdo_t *pdo;

   286

   287

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

   288

        if (pos--)

   289

            continue;

   290

        return pdo;

   291

    }

   292

   293

    return NULL;

   294

}

   295

   296

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

   297

   298

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

   299

 *

   300

 * \return Number of PDOs.

   301

 */

   302

unsigned int ec_pdo_list_count(

   303

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

   304

        )

   305

{

   306

    const ec_pdo_t *pdo;

   307

    unsigned int num = 0;

   308

   309

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

   310

        num++;

   311

    }

   312

   313

    return num;

   314

}

   315

   316

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

   317

   318

/** Outputs the PDOs in the list.

   319

 */

   320

void ec_pdo_list_print(

   321

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

   322

        )

   323

{

   324

    const ec_pdo_t *pdo;

   325

   326

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

   327

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

   328

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

   329

            printk(" ");

   330

    }

   331

}

   332

   333

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