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

   Methods of an EtherCAT datagram.

    37

*/

    38

    39

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

    40

    41

#include <linux/slab.h>

    42

    43

#include "datagram.h"

    44

#include "master.h"

    45

    46

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

    47

    48

/** \cond */

    49

    50

#define EC_FUNC_HEADER \

    51

    if (unlikely(ec_datagram_prealloc(datagram, data_size))) \

    52

        return -1; \

    53

    datagram->index = 0; \

    54

    datagram->working_counter = 0; \

    55

    datagram->state = EC_DATAGRAM_INIT;

    56

    57

#define EC_FUNC_FOOTER \

    58

    datagram->data_size = data_size; \

    59

    memset(datagram->data, 0x00, data_size); \

    60

    return 0;

    61

    62

/** \endcond */

    63

    64

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

    65

    66

/** Constructor.

    67

 */

    68

void ec_datagram_init(ec_datagram_t *datagram /**< EtherCAT datagram */)

    69

{

    70

    INIT_LIST_HEAD(&datagram->queue); // mark as unqueued

    71

    datagram->type = EC_DATAGRAM_NONE;

    72

    memset(datagram->address, 0x00, EC_ADDR_LEN);

    73

    datagram->data = NULL;

    74

    datagram->data_origin = EC_ORIG_INTERNAL;

    75

    datagram->mem_size = 0;

    76

    datagram->data_size = 0;

    77

    datagram->index = 0x00;

    78

    datagram->working_counter = 0x0000;

    79

    datagram->state = EC_DATAGRAM_INIT;

    80

    datagram->cycles_sent = 0;

    81

    datagram->jiffies_sent = 0;

    82

    datagram->cycles_received = 0;

    83

    datagram->jiffies_received = 0;

    84

    datagram->skip_count = 0;

    85

    datagram->stats_output_jiffies = 0;

    86

    memset(datagram->name, 0x00, EC_DATAGRAM_NAME_SIZE);

    87

}

    88

    89

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

    90

    91

/** Destructor.

    92

 */

    93

void ec_datagram_clear(ec_datagram_t *datagram /**< EtherCAT datagram */)

    94

{

    95

    if (datagram->data_origin == EC_ORIG_INTERNAL && datagram->data)

    96

        kfree(datagram->data);

    97

}

    98

    99

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

   100

   101

/** Allocates internal payload memory.

   102

 *

   103

 * If the allocated memory is already larger than requested, nothing ist done.

   104

 *

   105

 * \attention If external payload memory has been provided, no range checking

   106

 *            is done!

   107

 *

   108

 * \return 0 in case of success, else < 0

   109

 */

   110

int ec_datagram_prealloc(ec_datagram_t *datagram, /**< EtherCAT datagram */

   111

        size_t size /**< New size in bytes */

   112

        )

   113

{

   114

    if (datagram->data_origin == EC_ORIG_EXTERNAL

   115

            || size <= datagram->mem_size)

   116

        return 0;

   117

   118

    if (datagram->data) {

   119

        kfree(datagram->data);

   120

        datagram->data = NULL;

   121

        datagram->mem_size = 0;

   122

    }

   123

   124

    if (!(datagram->data = kmalloc(size, GFP_ATOMIC))) {

   125

        EC_ERR("Failed to allocate %i bytes of datagram memory!\n", size);

   126

        return -1;

   127

    }

   128

   129

    datagram->mem_size = size;

   130

    return 0;

   131

}

   132

   133

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

   134

   135

/** Initializes an EtherCAT NPRD datagram.

   136

 *

   137

 * Node-adressed physical read.

   138

 *

   139

 * \return 0 in case of success, else < 0

   140

 */

   141

int ec_datagram_nprd(ec_datagram_t *datagram,

   142

                     /**< EtherCAT datagram */

   143

                     uint16_t node_address,

   144

                     /**< configured station address */

   145

                     uint16_t offset,

   146

                     /**< physical memory address */

   147

                     size_t data_size

   148

                     /**< number of bytes to read */

   149

                     )

   150

{

   151

    if (unlikely(node_address == 0x0000))

   152

        EC_WARN("Using node address 0x0000!\n");

   153

   154

    EC_FUNC_HEADER;

   155

    datagram->type = EC_DATAGRAM_NPRD;

   156

    EC_WRITE_U16(datagram->address, node_address);

   157

    EC_WRITE_U16(datagram->address + 2, offset);

   158

    EC_FUNC_FOOTER;

   159

}

   160

   161

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

   162

   163

/** Initializes an EtherCAT NPWR datagram.

   164

 *

   165

 * Node-adressed physical write.

   166

 *

   167

 * \return 0 in case of success, else < 0

   168

 */

   169

int ec_datagram_npwr(ec_datagram_t *datagram,

   170

                     /**< EtherCAT datagram */

   171

                     uint16_t node_address,

   172

                     /**< configured station address */

   173

                     uint16_t offset,

   174

                     /**< physical memory address */

   175

                     size_t data_size

   176

                     /**< number of bytes to write */

   177

                     )

   178

{

   179

    if (unlikely(node_address == 0x0000))

   180

        EC_WARN("Using node address 0x0000!\n");

   181

   182

    EC_FUNC_HEADER;

   183

    datagram->type = EC_DATAGRAM_NPWR;

   184

    EC_WRITE_U16(datagram->address, node_address);

   185

    EC_WRITE_U16(datagram->address + 2, offset);

   186

    EC_FUNC_FOOTER;

   187

}

   188

   189

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

   190

   191

/** Initializes an EtherCAT APRD datagram.

   192

 *

   193

 * Autoincrement physical read.

   194

 *

   195

 * \return 0 in case of success, else < 0

   196

 */

   197

int ec_datagram_aprd(ec_datagram_t *datagram,

   198

                     /**< EtherCAT datagram */

   199

                     uint16_t ring_position,

   200

                     /**< auto-increment position */

   201

                     uint16_t offset,

   202

                     /**< physical memory address */

   203

                     size_t data_size

   204

                     /**< number of bytes to read */

   205

                     )

   206

{

   207

    EC_FUNC_HEADER;

   208

    datagram->type = EC_DATAGRAM_APRD;

   209

    EC_WRITE_S16(datagram->address, (int16_t) ring_position * (-1));

   210

    EC_WRITE_U16(datagram->address + 2, offset);

   211

    EC_FUNC_FOOTER;

   212

}

   213

   214

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

   215

   216

/** Initializes an EtherCAT APWR datagram.

   217

 *

   218

 * Autoincrement physical write.

   219

 *

   220

 * \return 0 in case of success, else < 0

   221

 */

   222

int ec_datagram_apwr(ec_datagram_t *datagram,

   223

                     /**< EtherCAT datagram */

   224

                     uint16_t ring_position,

   225

                     /**< auto-increment position */

   226

                     uint16_t offset,

   227

                     /**< physical memory address */

   228

                     size_t data_size

   229

                     /**< number of bytes to write */

   230

                     )

   231

{

   232

    EC_FUNC_HEADER;

   233

    datagram->type = EC_DATAGRAM_APWR;

   234

    EC_WRITE_S16(datagram->address, (int16_t) ring_position * (-1));

   235

    EC_WRITE_U16(datagram->address + 2, offset);

   236

    EC_FUNC_FOOTER;

   237

}

   238

   239

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

   240

   241

/** Initializes an EtherCAT BRD datagram.

   242

 *

   243

 * Broadcast read.

   244

 *

   245

 * \return 0 in case of success, else < 0

   246

 */

   247

int ec_datagram_brd(ec_datagram_t *datagram,

   248

                    /**< EtherCAT datagram */

   249

                    uint16_t offset,

   250

                    /**< physical memory address */

   251

                    size_t data_size

   252

                    /**< number of bytes to read */

   253

                    )

   254

{

   255

    EC_FUNC_HEADER;

   256

    datagram->type = EC_DATAGRAM_BRD;

   257

    EC_WRITE_U16(datagram->address, 0x0000);

   258

    EC_WRITE_U16(datagram->address + 2, offset);

   259

    EC_FUNC_FOOTER;

   260

}

   261

   262

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

   263

   264

/** Initializes an EtherCAT BWR datagram.

   265

 *

   266

 * Broadcast write.

   267

 *

   268

 * \return 0 in case of success, else < 0

   269

 */

   270

int ec_datagram_bwr(ec_datagram_t *datagram,

   271

                    /**< EtherCAT datagram */

   272

                    uint16_t offset,

   273

                    /**< physical memory address */

   274

                    size_t data_size

   275

                    /**< number of bytes to write */

   276

                    )

   277

{

   278

    EC_FUNC_HEADER;

   279

    datagram->type = EC_DATAGRAM_BWR;

   280

    EC_WRITE_U16(datagram->address, 0x0000);

   281

    EC_WRITE_U16(datagram->address + 2, offset);

   282

    EC_FUNC_FOOTER;

   283

}

   284

   285

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

   286

   287

/** Initializes an EtherCAT LRW datagram.

   288

 *

   289

 * Logical read write.

   290

 *

   291

 * \attention It is assumed, that the external memory is at least \a data_size

   292

 *            bytes large.

   293

 *

   294

 * \return 0 in case of success, else < 0

   295

 */

   296

int ec_datagram_lrw(

   297

        ec_datagram_t *datagram, /**< EtherCAT datagram */

   298

        uint32_t offset, /**< Logical address. */

   299

        size_t data_size, /**< Number of bytes to read/write. */

   300

        uint8_t *external_memory /**< Pointer to the memory to use. */

   301

        )

   302

{

   303

    datagram->data = external_memory;

   304

    datagram->data_origin = EC_ORIG_EXTERNAL;

   305

    EC_FUNC_HEADER;

   306

    datagram->type = EC_DATAGRAM_LRW;

   307

    EC_WRITE_U32(datagram->address, offset);

   308

    EC_FUNC_FOOTER;

   309

}

   310

   311

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

   312

   313

/** Evaluates the working counter of a single-cast datagram.

   314

 *

   315

 * Outputs an error message.

   316

 */

   317

void ec_datagram_print_wc_error(

   318

        const ec_datagram_t *datagram /**< EtherCAT datagram */

   319

        )

   320

{

   321

    if (datagram->working_counter == 0)

   322

        printk("No response.");

   323

    else if (datagram->working_counter > 1)

   324

        printk("%u slaves responded!", datagram->working_counter);

   325

    else

   326

        printk("Success.");

   327

    printk("\n");

   328

}

   329

   330

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

   331

   332

/** Outputs datagram statistics at most every second.

   333

 */

   334

void ec_datagram_output_stats(

   335

        ec_datagram_t *datagram

   336

        )

   337

{

   338

    if (jiffies - datagram->stats_output_jiffies < HZ) {

   339

        datagram->stats_output_jiffies = jiffies;

   340

   341

        if (unlikely(datagram->skip_count)) {

   342

            EC_WARN("Datagram %x (%s) was SKIPPED %u time%s.\n",

   343

                    (unsigned int) datagram, datagram->name,

   344

                    datagram->skip_count,

   345

                    datagram->skip_count == 1 ? "" : "s");

   346

            datagram->skip_count = 0;

   347

        }

   348

    }

   349

}

   350

   351

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