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

    33

*/

    34

    35

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

    36

    37

#include <linux/module.h>

    38

    39

#include "globals.h"

    40

#include "pdo.h"

    41

#include "slave_config.h"

    42

#include "master.h"

    43

    44

#include "pdo_list.h"

    45

    46

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

    47

    48

/** PDO list constructor.

    49

 */

    50

void ec_pdo_list_init(

    51

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

    52

        )

    53

{

    54

    INIT_LIST_HEAD(&pl->list);

    55

}

    56

    57

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

    58

    59

/** PDO list destructor.

    60

 */

    61

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

    62

{

    63

    ec_pdo_list_clear_pdos(pl);

    64

}

    65

    66

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

    67

    68

/** Clears the list of mapped PDOs.

    69

 */

    70

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

    71

{

    72

    ec_pdo_t *pdo, *next;

    73

    74

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

    75

        list_del_init(&pdo->list);

    76

        ec_pdo_clear(pdo);

    77

        kfree(pdo);

    78

    }

    79

}

    80

    81

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

    82

    83

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

    84

 *

    85

 * \retval Data size in byte.

    86

 */

    87

uint16_t ec_pdo_list_total_size(

    88

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

    89

        )

    90

{

    91

    unsigned int bit_size;

    92

    const ec_pdo_t *pdo;

    93

    const ec_pdo_entry_t *pdo_entry;

    94

    uint16_t byte_size;

    95

    96

    bit_size = 0;

    97

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

    98

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

    99

            bit_size += pdo_entry->bit_length;

   100

        }

   101

    }

   102

   103

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

   104

        byte_size = bit_size / 8 + 1;

   105

    else

   106

        byte_size = bit_size / 8;

   107

   108

    return byte_size;

   109

}

   110

   111

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

   112

   113

/** Add a new PDO to the list.

   114

 *

   115

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

   116

 */

   117

ec_pdo_t *ec_pdo_list_add_pdo(

   118

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

   119

        uint16_t index /**< PDO index. */

   120

        )

   121

{

   122

    ec_pdo_t *pdo;

   123

   124

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

   125

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

   126

        return ERR_PTR(-ENOMEM);

   127

    }

   128

   129

    ec_pdo_init(pdo);

   130

    pdo->index = index;

   131

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

   132

    return pdo;

   133

}

   134

   135

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

   136

   137

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

   138

 *

   139

 * \return 0 on success, else < 0

   140

 */

   141

int ec_pdo_list_add_pdo_copy(

   142

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

   143

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

   144

        )

   145

{

   146

    ec_pdo_t *mapped_pdo;

   147

    int ret;

   148

   149

    // PDO already mapped?

   150

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

   151

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

   152

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

   153

        return -EEXIST;

   154

    }

   155

   156

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

   157

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

   158

        return -ENOMEM;

   159

    }

   160

   161

    ret = ec_pdo_init_copy(mapped_pdo, pdo);

   162

    if (ret < 0) {

   163

        kfree(mapped_pdo);

   164

        return ret;

   165

    }

   166

   167

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

   168

    return 0;

   169

}

   170

   171

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

   172

   173

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

   174

 *

   175

 * \return 0 on success, else < 0

   176

 */

   177

int ec_pdo_list_copy(

   178

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

   179

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

   180

        )

   181

{

   182

    ec_pdo_t *other_pdo;

   183

    int ret;

   184

   185

    ec_pdo_list_clear_pdos(pl);

   186

   187

    // PDO already mapped?

   188

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

   189

        ret = ec_pdo_list_add_pdo_copy(pl, other_pdo);

   190

        if (ret)

   191

            return ret;

   192

    }

   193

   194

    return 0;

   195

}

   196

   197

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

   198

   199

/** Compares two PDO lists.

   200

 *

   201

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

   202

 * mapping).

   203

 *

   204

 * \retval 1 The given PDO lists are equal.

   205

 * \retval 0 The given PDO lists differ.

   206

 */

   207

int ec_pdo_list_equal(

   208

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

   209

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

   210

        )

   211

{

   212

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

   213

    const ec_pdo_t *p1, *p2;

   214

   215

    h1 = l1 = &pl1->list;

   216

    h2 = l2 = &pl2->list;

   217

   218

    while (1) {

   219

        l1 = l1->next;

   220

        l2 = l2->next;

   221

   222

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

   223

            return 0;

   224

        if (l1 == h1) // both finished

   225

            break;

   226

   227

        p1 = list_entry(l1, ec_pdo_t, list);

   228

        p2 = list_entry(l2, ec_pdo_t, list);

   229

   230

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

   231

            return 0;

   232

    }

   233

   234

    return 1;

   235

}

   236

   237

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

   238

   239

/** Finds a PDO with the given index.

   240

 */

   241

ec_pdo_t *ec_pdo_list_find_pdo(

   242

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

   243

        uint16_t index /**< PDO index. */

   244

        )

   245

{

   246

    ec_pdo_t *pdo;

   247

   248

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

   249

        if (pdo->index != index)

   250

            continue;

   251

        return pdo;

   252

    }

   253

   254

    return NULL;

   255

}

   256

   257

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

   258

   259

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

   260

 */

   261

const ec_pdo_t *ec_pdo_list_find_pdo_const(

   262

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

   263

        uint16_t index /**< PDO index. */

   264

        )

   265

{

   266

    const ec_pdo_t *pdo;

   267

   268

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

   269

        if (pdo->index != index)

   270

            continue;

   271

        return pdo;

   272

    }

   273

   274

    return NULL;

   275

}

   276

   277

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

   278

   279

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

   280

 *

   281

 * Const version.

   282

 */

   283

const ec_pdo_t *ec_pdo_list_find_pdo_by_pos_const(

   284

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

   285

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

   286

        )

   287

{

   288

    const ec_pdo_t *pdo;

   289

   290

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

   291

        if (pos--)

   292

            continue;

   293

        return pdo;

   294

    }

   295

   296

    return NULL;

   297

}

   298

   299

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

   300

   301

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

   302

 *

   303

 * \return Number of PDOs.

   304

 */

   305

unsigned int ec_pdo_list_count(

   306

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

   307

        )

   308

{

   309

    const ec_pdo_t *pdo;

   310

    unsigned int num = 0;

   311

   312

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

   313

        num++;

   314

    }

   315

   316

    return num;

   317

}

   318

   319

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

   320

   321

/** Outputs the PDOs in the list.

   322

 */

   323

void ec_pdo_list_print(

   324

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

   325

        )

   326

{

   327

    const ec_pdo_t *pdo;

   328

   329

    if (list_empty(&pl->list)) {

   330

        printk("(none)");

   331

    } else {

   332

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

   333

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

   334

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

   335

                printk(" ");

   336

        }

   337

    }

   338

}

   339

   340

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