1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006  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

    10

 *  and/or modify it under the terms of the GNU General Public License

    11

 *  as published by the Free Software Foundation; either version 2 of the

    12

 *  License, or (at your option) any later version.

    13

 *

    14

 *  The IgH EtherCAT Master is distributed in the hope that it will be

    15

 *  useful, but WITHOUT ANY WARRANTY; without even the implied warranty of

    16

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    17

 *  GNU General Public License for more details.

    18

 *

    19

 *  You should have received a copy of the GNU General Public License

    20

 *  along with the IgH EtherCAT Master; if not, write to the Free Software

    21

 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    22

 *

    23

 *  The right to use EtherCAT Technology is granted and comes free of

    24

 *  charge under condition of compatibility of product made by

    25

 *  Licensee. People intending to distribute/sell products based on the

    26

 *  code, have to sign an agreement to guarantee that products using

    27

 *  software based on IgH EtherCAT master stay compatible with the actual

    28

 *  EtherCAT specification (which are released themselves as an open

    29

 *  standard) as the (only) precondition to have the right to use EtherCAT

    30

 *  Technology, IP and trade marks.

    31

 *

    32

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

    33

    34

/**

    35

   \file

    36

   EtherCAT Pdo list methods.

    37

*/

    38

    39

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

    40

    41

#include <linux/module.h>

    42

    43

#include "globals.h"

    44

#include "pdo.h"

    45

#include "slave_config.h"

    46

#include "master.h"

    47

    48

#include "pdo_list.h"

    49

    50

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

    51

    52

/** Pdo list constructor.

    53

 */

    54

void ec_pdo_list_init(

    55

        ec_pdo_list_t *pl /**< Pdo list. */

    56

        )

    57

{

    58

    INIT_LIST_HEAD(&pl->list);

    59

}

    60

    61

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

    62

    63

/** Pdo list destructor.

    64

 */

    65

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

    66

{

    67

    ec_pdo_list_clear_pdos(pl);

    68

}

    69

    70

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

    71

    72

/** Clears the list of mapped Pdos.

    73

 */

    74

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

    75

{

    76

    ec_pdo_t *pdo, *next;

    77

    78

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

    79

        list_del_init(&pdo->list);

    80

        ec_pdo_clear(pdo);

    81

        kfree(pdo);

    82

    }

    83

}

    84

    85

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

    86

    87

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

    88

 *

    89

 * \retval Data size in byte.

    90

 */

    91

uint16_t ec_pdo_list_total_size(

    92

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

    93

        )

    94

{

    95

    unsigned int bit_size;

    96

    const ec_pdo_t *pdo;

    97

    const ec_pdo_entry_t *pdo_entry;

    98

    uint16_t byte_size;

    99

   100

    bit_size = 0;

   101

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

   102

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

   103

            bit_size += pdo_entry->bit_length;

   104

        }

   105

    }

   106

   107

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

   108

        byte_size = bit_size / 8 + 1;

   109

    else

   110

        byte_size = bit_size / 8;

   111

   112

    return byte_size;

   113

}

   114

   115

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

   116

   117

/** Add a new Pdo to the list.

   118

 *

   119

 * \retval >0 Pointer to new Pdo.

   120

 * \retval NULL No memory.

   121

 */

   122

ec_pdo_t *ec_pdo_list_add_pdo(

   123

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

   124

        ec_direction_t dir, /**< Direction. */

   125

        uint16_t index /**< Pdo index. */

   126

        )

   127

{

   128

    ec_pdo_t *pdo;

   129

   130

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

   131

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

   132

        return NULL;

   133

    }

   134

   135

    ec_pdo_init(pdo);

   136

    pdo->dir = dir;

   137

    pdo->index = index;

   138

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

   139

    return pdo;

   140

}

   141

   142

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

   143

   144

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

   145

 *

   146

 * \return 0 on success, else < 0

   147

 */

   148

int ec_pdo_list_add_pdo_copy(

   149

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

   150

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

   151

        )

   152

{

   153

    ec_pdo_t *mapped_pdo;

   154

   155

    // Pdo already mapped?

   156

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

   157

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

   158

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

   159

        return -1;

   160

    }

   161

   162

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

   163

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

   164

        return -1;

   165

    }

   166

   167

    if (ec_pdo_init_copy(mapped_pdo, pdo)) {

   168

        kfree(mapped_pdo);

   169

        return -1;

   170

    }

   171

   172

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

   173

    return 0;

   174

}

   175

   176

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

   177

   178

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

   179

 *

   180

 * \return 0 on success, else < 0

   181

 */

   182

int ec_pdo_list_copy(

   183

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

   184

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

   185

        )

   186

{

   187

    ec_pdo_t *other_pdo;

   188

   189

    ec_pdo_list_clear_pdos(pl);

   190

   191

    // Pdo already mapped?

   192

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

   193

        if (ec_pdo_list_add_pdo_copy(pl, other_pdo))

   194

            return -1;

   195

    }

   196

   197

    return 0;

   198

}

   199

   200

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

   201

   202

/** Compares two Pdo lists.

   203

 *

   204

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

   205

 * configuration).

   206

 *

   207

 * \retval 1 The given Pdo lists are equal.

   208

 * \retval 0 The given Pdo lists differ.

   209

 */

   210

int ec_pdo_list_equal(

   211

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

   212

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

   213

        )

   214

{

   215

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

   216

    const ec_pdo_t *p1, *p2;

   217

   218

    h1 = l1 = &pl1->list;

   219

    h2 = l2 = &pl2->list;

   220

   221

    while (1) {

   222

        l1 = l1->next;

   223

        l2 = l2->next;

   224

   225

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

   226

            return 0;

   227

        if (l1 == h1) // both finished

   228

            break;

   229

   230

        p1 = list_entry(l1, ec_pdo_t, list);

   231

        p2 = list_entry(l2, ec_pdo_t, list);

   232

   233

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

   234

            return 0;

   235

    }

   236

   237

    return 1;

   238

}

   239

   240

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

   241

   242

/** Finds a Pdo with the given index.

   243

 */

   244

ec_pdo_t *ec_pdo_list_find_pdo(

   245

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

   246

        uint16_t index /**< Pdo index. */

   247

        )

   248

{

   249

    ec_pdo_t *pdo;

   250

   251

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

   252

        if (pdo->index != index)

   253

            continue;

   254

        return pdo;

   255

    }

   256

   257

    return NULL;

   258

}

   259

   260

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

   261

   262

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

   263

 */

   264

const ec_pdo_t *ec_pdo_list_find_pdo_const(

   265

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

   266

        uint16_t index /**< Pdo index. */

   267

        )

   268

{

   269

    const ec_pdo_t *pdo;

   270

   271

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

   272

        if (pdo->index != index)

   273

            continue;

   274

        return pdo;

   275

    }

   276

   277

    return NULL;

   278

}

   279

   280

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

   281

   282

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

   283

 *

   284

 * Const version.

   285

 */

   286

const ec_pdo_t *ec_pdo_list_find_pdo_by_pos_const(

   287

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

   288

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

   289

        )

   290

{

   291

    const ec_pdo_t *pdo;

   292

   293

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

   294

        if (pos--)

   295

            continue;

   296

        return pdo;

   297

    }

   298

   299

    return NULL;

   300

}

   301

   302

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

   303

   304

/**

   305

 */

   306

unsigned int ec_pdo_list_count(

   307

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

   308

        )

   309

{

   310

    const ec_pdo_t *pdo;

   311

    unsigned int num = 0;

   312

   313

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

   314

        num++;

   315

    }

   316

   317

    return num;

   318

}

   319

   320

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