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

        uint16_t index /**< Pdo index. */

   125

        )

   126

{

   127

    ec_pdo_t *pdo;

   128

   129

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

   130

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

   131

        return NULL;

   132

    }

   133

   134

    ec_pdo_init(pdo);

   135

    pdo->index = index;

   136

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

   137

    return pdo;

   138

}

   139

   140

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

   141

   142

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

   143

 *

   144

 * \return 0 on success, else < 0

   145

 */

   146

int ec_pdo_list_add_pdo_copy(

   147

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

   148

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

   149

        )

   150

{

   151

    ec_pdo_t *mapped_pdo;

   152

   153

    // Pdo already mapped?

   154

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

   155

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

   156

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

   157

        return -1;

   158

    }

   159

   160

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

   161

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

   162

        return -1;

   163

    }

   164

   165

    if (ec_pdo_init_copy(mapped_pdo, pdo)) {

   166

        kfree(mapped_pdo);

   167

        return -1;

   168

    }

   169

   170

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

   171

    return 0;

   172

}

   173

   174

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

   175

   176

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

   177

 *

   178

 * \return 0 on success, else < 0

   179

 */

   180

int ec_pdo_list_copy(

   181

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

   182

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

   183

        )

   184

{

   185

    ec_pdo_t *other_pdo;

   186

   187

    ec_pdo_list_clear_pdos(pl);

   188

   189

    // Pdo already mapped?

   190

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

   191

        if (ec_pdo_list_add_pdo_copy(pl, other_pdo))

   192

            return -1;

   193

    }

   194

   195

    return 0;

   196

}

   197

   198

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

   199

   200

/** Compares two Pdo lists.

   201

 *

   202

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

   203

 * mapping).

   204

 *

   205

 * \retval 1 The given Pdo lists are equal.

   206

 * \retval 0 The given Pdo lists differ.

   207

 */

   208

int ec_pdo_list_equal(

   209

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

   210

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

   211

        )

   212

{

   213

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

   214

    const ec_pdo_t *p1, *p2;

   215

   216

    h1 = l1 = &pl1->list;

   217

    h2 = l2 = &pl2->list;

   218

   219

    while (1) {

   220

        l1 = l1->next;

   221

        l2 = l2->next;

   222

   223

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

   224

            return 0;

   225

        if (l1 == h1) // both finished

   226

            break;

   227

   228

        p1 = list_entry(l1, ec_pdo_t, list);

   229

        p2 = list_entry(l2, ec_pdo_t, list);

   230

   231

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

   232

            return 0;

   233

    }

   234

   235

    return 1;

   236

}

   237

   238

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

   239

   240

/** Finds a Pdo with the given index.

   241

 */

   242

ec_pdo_t *ec_pdo_list_find_pdo(

   243

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

   244

        uint16_t index /**< Pdo index. */

   245

        )

   246

{

   247

    ec_pdo_t *pdo;

   248

   249

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

   250

        if (pdo->index != index)

   251

            continue;

   252

        return pdo;

   253

    }

   254

   255

    return NULL;

   256

}

   257

   258

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

   259

   260

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

   261

 */

   262

const ec_pdo_t *ec_pdo_list_find_pdo_const(

   263

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

   264

        uint16_t index /**< Pdo index. */

   265

        )

   266

{

   267

    const ec_pdo_t *pdo;

   268

   269

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

   270

        if (pdo->index != index)

   271

            continue;

   272

        return pdo;

   273

    }

   274

   275

    return NULL;

   276

}

   277

   278

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

   279

   280

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

   281

 *

   282

 * Const version.

   283

 */

   284

const ec_pdo_t *ec_pdo_list_find_pdo_by_pos_const(

   285

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

   286

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

   287

        )

   288

{

   289

    const ec_pdo_t *pdo;

   290

   291

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

   292

        if (pos--)

   293

            continue;

   294

        return pdo;

   295

    }

   296

   297

    return NULL;

   298

}

   299

   300

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

   301

   302

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

   303

 *

   304

 * \return Number of Pdos.

   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

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

   321

   322

/** Outputs the Pdos in the list.

   323

 */

   324

void ec_pdo_list_print(

   325

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

   326

        )

   327

{

   328

    const ec_pdo_t *pdo;

   329

   330

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

   331

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

   332

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

   333

            printk(" ");

   334

    }

   335

}

   336

   337

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