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

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

   120

 */

   121

ec_pdo_t *ec_pdo_list_add_pdo(

   122

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

   123

        uint16_t index /**< Pdo index. */

   124

        )

   125

{

   126

    ec_pdo_t *pdo;

   127

   128

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

   129

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

   130

        return ERR_PTR(-ENOMEM);

   131

    }

   132

   133

    ec_pdo_init(pdo);

   134

    pdo->index = index;

   135

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

   136

    return pdo;

   137

}

   138

   139

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

   140

   141

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

   142

 *

   143

 * \return 0 on success, else < 0

   144

 */

   145

int ec_pdo_list_add_pdo_copy(

   146

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

   147

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

   148

        )

   149

{

   150

    ec_pdo_t *mapped_pdo;

   151

    int ret;

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

   158

    }

   159

   160

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

   161

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

   162

        return -ENOMEM;

   163

    }

   164

   165

    ret = ec_pdo_init_copy(mapped_pdo, pdo);

   166

    if (ret < 0) {

   167

        kfree(mapped_pdo);

   168

        return ret;

   169

    }

   170

   171

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

   172

    return 0;

   173

}

   174

   175

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

   176

   177

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

   178

 *

   179

 * \return 0 on success, else < 0

   180

 */

   181

int ec_pdo_list_copy(

   182

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

   183

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

   184

        )

   185

{

   186

    ec_pdo_t *other_pdo;

   187

    int ret;

   188

   189

    ec_pdo_list_clear_pdos(pl);

   190

   191

    // Pdo already mapped?

   192

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

   193

        ret = ec_pdo_list_add_pdo_copy(pl, other_pdo);

   194

        if (ret)

   195

            return ret;

   196

    }

   197

   198

    return 0;

   199

}

   200

   201

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

   202

   203

/** Compares two Pdo lists.

   204

 *

   205

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

   206

 * mapping).

   207

 *

   208

 * \retval 1 The given Pdo lists are equal.

   209

 * \retval 0 The given Pdo lists differ.

   210

 */

   211

int ec_pdo_list_equal(

   212

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

   213

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

   214

        )

   215

{

   216

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

   217

    const ec_pdo_t *p1, *p2;

   218

   219

    h1 = l1 = &pl1->list;

   220

    h2 = l2 = &pl2->list;

   221

   222

    while (1) {

   223

        l1 = l1->next;

   224

        l2 = l2->next;

   225

   226

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

   227

            return 0;

   228

        if (l1 == h1) // both finished

   229

            break;

   230

   231

        p1 = list_entry(l1, ec_pdo_t, list);

   232

        p2 = list_entry(l2, ec_pdo_t, list);

   233

   234

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

   235

            return 0;

   236

    }

   237

   238

    return 1;

   239

}

   240

   241

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

   242

   243

/** Finds a Pdo with the given index.

   244

 */

   245

ec_pdo_t *ec_pdo_list_find_pdo(

   246

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

   247

        uint16_t index /**< Pdo index. */

   248

        )

   249

{

   250

    ec_pdo_t *pdo;

   251

   252

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

   253

        if (pdo->index != index)

   254

            continue;

   255

        return pdo;

   256

    }

   257

   258

    return NULL;

   259

}

   260

   261

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

   262

   263

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

   264

 */

   265

const ec_pdo_t *ec_pdo_list_find_pdo_const(

   266

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

   267

        uint16_t index /**< Pdo index. */

   268

        )

   269

{

   270

    const ec_pdo_t *pdo;

   271

   272

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

   273

        if (pdo->index != index)

   274

            continue;

   275

        return pdo;

   276

    }

   277

   278

    return NULL;

   279

}

   280

   281

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

   282

   283

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

   284

 *

   285

 * Const version.

   286

 */

   287

const ec_pdo_t *ec_pdo_list_find_pdo_by_pos_const(

   288

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

   289

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

   290

        )

   291

{

   292

    const ec_pdo_t *pdo;

   293

   294

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

   295

        if (pos--)

   296

            continue;

   297

        return pdo;

   298

    }

   299

   300

    return NULL;

   301

}

   302

   303

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

   304

   305

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

   306

 *

   307

 * \return Number of Pdos.

   308

 */

   309

unsigned int ec_pdo_list_count(

   310

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

   311

        )

   312

{

   313

    const ec_pdo_t *pdo;

   314

    unsigned int num = 0;

   315

   316

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

   317

        num++;

   318

    }

   319

   320

    return num;

   321

}

   322

   323

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

   324

   325

/** Outputs the Pdos in the list.

   326

 */

   327

void ec_pdo_list_print(

   328

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

   329

        )

   330

{

   331

    const ec_pdo_t *pdo;

   332

   333

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

   334

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

   335

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

   336

            printk(" ");

   337

    }

   338

}

   339

   340

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