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

 *  Using the EtherCAT technology and brand is permitted in compliance with

    23

 *  the industrial property and similar rights of Beckhoff Automation GmbH.

    24

 *

    25

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

    26

    27

/**

    28

   \file

    29

   Methods of an EtherCAT datagram.

    30

*/

    31

    32

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

    33

    34

#include <linux/slab.h>

    35

    36

#include "datagram.h"

    37

#include "master.h"

    38

    39

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

    40

    41

/** \cond */

    42

    43

#define EC_FUNC_HEADER \

    44

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

    45

        return -1; \

    46

    datagram->index = 0; \

    47

    datagram->working_counter = 0; \

    48

    datagram->state = EC_DATAGRAM_INIT;

    49

    50

#define EC_FUNC_FOOTER \

    51

    datagram->data_size = data_size; \

    52

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

    53

    return 0;

    54

    55

/** \endcond */

    56

    57

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

    58

    59

/** Array of datagram type strings used in ec_datagram_type_string().

    60

 *

    61

 * \attention This is indexed by ec_datagram_type_t.

    62

 */

    63

static const char *type_strings[] = {

    64

    "?",

    65

    "APRD",

    66

    "APWR",

    67

    "APRW",

    68

    "FPRD",

    69

    "FPWR",

    70

    "FPRW",

    71

    "BRD",

    72

    "BWR",

    73

    "BRW",

    74

    "LRD",

    75

    "LWR",

    76

    "LRW",

    77

    "ARMW",

    78

    "FRMW"

    79

};

    80

    81

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

    82

    83

/** Constructor.

    84

 */

    85

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

    86

{

    87

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

    88

    datagram->type = EC_DATAGRAM_NONE;

    89

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

    90

    datagram->data = NULL;

    91

    datagram->data_origin = EC_ORIG_INTERNAL;

    92

    datagram->mem_size = 0;

    93

    datagram->data_size = 0;

    94

    datagram->index = 0x00;

    95

    datagram->working_counter = 0x0000;

    96

    datagram->state = EC_DATAGRAM_INIT;

    97

#ifdef EC_HAVE_CYCLES

    98

    datagram->cycles_sent = 0;

    99

#endif

   100

    datagram->jiffies_sent = 0;

   101

#ifdef EC_HAVE_CYCLES

   102

    datagram->cycles_received = 0;

   103

#endif

   104

    datagram->jiffies_received = 0;

   105

    datagram->skip_count = 0;

   106

    datagram->stats_output_jiffies = 0;

   107

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

   108

}

   109

   110

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

   111

   112

/** Destructor.

   113

 */

   114

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

   115

{

   116

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

   117

        kfree(datagram->data);

   118

}

   119

   120

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

   121

   122

/** Allocates internal payload memory.

   123

 *

   124

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

   125

 *

   126

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

   127

 *            is done!

   128

 *

   129

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

   130

 */

   131

int ec_datagram_prealloc(

   132

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

   133

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

   134

        )

   135

{

   136

    if (datagram->data_origin == EC_ORIG_EXTERNAL

   137

            || size <= datagram->mem_size)

   138

        return 0;

   139

   140

    if (datagram->data) {

   141

        kfree(datagram->data);

   142

        datagram->data = NULL;

   143

        datagram->mem_size = 0;

   144

    }

   145

   146

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

   147

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

   148

        return -1;

   149

    }

   150

   151

    datagram->mem_size = size;

   152

    return 0;

   153

}

   154

   155

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

   156

   157

/** Initializes an EtherCAT APRD datagram.

   158

 *

   159

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

   160

 */

   161

int ec_datagram_aprd(

   162

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

   163

        uint16_t ring_position, /**< Auto-increment address. */

   164

        uint16_t mem_address, /**< Physical memory address. */

   165

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

   166

        )

   167

{

   168

    EC_FUNC_HEADER;

   169

    datagram->type = EC_DATAGRAM_APRD;

   170

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

   171

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

   172

    EC_FUNC_FOOTER;

   173

}

   174

   175

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

   176

   177

/** Initializes an EtherCAT APWR datagram.

   178

 *

   179

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

   180

 */

   181

int ec_datagram_apwr(

   182

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

   183

        uint16_t ring_position, /**< Auto-increment address. */

   184

        uint16_t mem_address, /**< Physical memory address. */

   185

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

   186

        )

   187

{

   188

    EC_FUNC_HEADER;

   189

    datagram->type = EC_DATAGRAM_APWR;

   190

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

   191

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

   192

    EC_FUNC_FOOTER;

   193

}

   194

   195

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

   196

   197

/** Initializes an EtherCAT APRW datagram.

   198

 *

   199

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

   200

 */

   201

int ec_datagram_aprw(

   202

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

   203

        uint16_t ring_position, /**< Auto-increment address. */

   204

        uint16_t mem_address, /**< Physical memory address. */

   205

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

   206

        )

   207

{

   208

    EC_FUNC_HEADER;

   209

    datagram->type = EC_DATAGRAM_APRW;

   210

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

   211

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

   212

    EC_FUNC_FOOTER;

   213

}

   214

   215

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

   216

   217

/** Initializes an EtherCAT ARMW datagram.

   218

 *

   219

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

   220

 */

   221

int ec_datagram_armw(

   222

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

   223

        uint16_t ring_position, /**< Auto-increment address. */

   224

        uint16_t mem_address, /**< Physical memory address. */

   225

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

   226

        )

   227

{

   228

    EC_FUNC_HEADER;

   229

    datagram->type = EC_DATAGRAM_ARMW;

   230

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

   231

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

   232

    EC_FUNC_FOOTER;

   233

}

   234

   235

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

   236

   237

/** Initializes an EtherCAT FPRD datagram.

   238

 *

   239

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

   240

 */

   241

int ec_datagram_fprd(

   242

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

   243

        uint16_t configured_address, /**< Configured station address. */

   244

        uint16_t mem_address, /**< Physical memory address. */

   245

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

   246

        )

   247

{

   248

    if (unlikely(configured_address == 0x0000))

   249

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

   250

   251

    EC_FUNC_HEADER;

   252

    datagram->type = EC_DATAGRAM_FPRD;

   253

    EC_WRITE_U16(datagram->address, configured_address);

   254

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

   255

    EC_FUNC_FOOTER;

   256

}

   257

   258

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

   259

   260

/** Initializes an EtherCAT FPWR datagram.

   261

 *

   262

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

   263

 */

   264

int ec_datagram_fpwr(

   265

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

   266

        uint16_t configured_address, /**< Configured station address. */

   267

        uint16_t mem_address, /**< Physical memory address. */

   268

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

   269

        )

   270

{

   271

    if (unlikely(configured_address == 0x0000))

   272

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

   273

   274

    EC_FUNC_HEADER;

   275

    datagram->type = EC_DATAGRAM_FPWR;

   276

    EC_WRITE_U16(datagram->address, configured_address);

   277

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

   278

    EC_FUNC_FOOTER;

   279

}

   280

   281

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

   282

   283

/** Initializes an EtherCAT FPRW datagram.

   284

 *

   285

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

   286

 */

   287

int ec_datagram_fprw(

   288

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

   289

        uint16_t configured_address, /**< Configured station address. */

   290

        uint16_t mem_address, /**< Physical memory address. */

   291

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

   292

        )

   293

{

   294

    if (unlikely(configured_address == 0x0000))

   295

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

   296

   297

    EC_FUNC_HEADER;

   298

    datagram->type = EC_DATAGRAM_FPRW;

   299

    EC_WRITE_U16(datagram->address, configured_address);

   300

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

   301

    EC_FUNC_FOOTER;

   302

}

   303

   304

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

   305

   306

/** Initializes an EtherCAT FRMW datagram.

   307

 *

   308

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

   309

 */

   310

int ec_datagram_frmw(

   311

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

   312

        uint16_t configured_address, /**< Configured station address. */

   313

        uint16_t mem_address, /**< Physical memory address. */

   314

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

   315

        )

   316

{

   317

    if (unlikely(configured_address == 0x0000))

   318

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

   319

   320

    EC_FUNC_HEADER;

   321

    datagram->type = EC_DATAGRAM_FRMW;

   322

    EC_WRITE_U16(datagram->address, configured_address);

   323

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

   324

    EC_FUNC_FOOTER;

   325

}

   326

   327

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

   328

   329

/** Initializes an EtherCAT BRD datagram.

   330

 *

   331

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

   332

 */

   333

int ec_datagram_brd(

   334

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

   335

        uint16_t mem_address, /**< Physical memory address. */

   336

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

   337

        )

   338

{

   339

    EC_FUNC_HEADER;

   340

    datagram->type = EC_DATAGRAM_BRD;

   341

    EC_WRITE_U16(datagram->address, 0x0000);

   342

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

   343

    EC_FUNC_FOOTER;

   344

}

   345

   346

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

   347

   348

/** Initializes an EtherCAT BWR datagram.

   349

 *

   350

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

   351

 */

   352

int ec_datagram_bwr(

   353

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

   354

        uint16_t mem_address, /**< Physical memory address. */

   355

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

   356

        )

   357

{

   358

    EC_FUNC_HEADER;

   359

    datagram->type = EC_DATAGRAM_BWR;

   360

    EC_WRITE_U16(datagram->address, 0x0000);

   361

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

   362

    EC_FUNC_FOOTER;

   363

}

   364

   365

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

   366

   367

/** Initializes an EtherCAT BRW datagram.

   368

 *

   369

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

   370

 */

   371

int ec_datagram_brw(

   372

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

   373

        uint16_t mem_address, /**< Physical memory address. */

   374

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

   375

        )

   376

{

   377

    EC_FUNC_HEADER;

   378

    datagram->type = EC_DATAGRAM_BRW;

   379

    EC_WRITE_U16(datagram->address, 0x0000);

   380

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

   381

    EC_FUNC_FOOTER;

   382

}

   383

   384

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

   385

   386

/** Initializes an EtherCAT LRD datagram.

   387

 *

   388

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

   389

 *            bytes large.

   390

 *

   391

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

   392

 */

   393

int ec_datagram_lrd(

   394

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

   395

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

   396

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

   397

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

   398

        )

   399

{

   400

    datagram->data = external_memory;

   401

    datagram->data_origin = EC_ORIG_EXTERNAL;

   402

    EC_FUNC_HEADER;

   403

    datagram->type = EC_DATAGRAM_LRD;

   404

    EC_WRITE_U32(datagram->address, offset);

   405

    EC_FUNC_FOOTER;

   406

}

   407

   408

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

   409

   410

/** Initializes an EtherCAT LWR datagram.

   411

 *

   412

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

   413

 *            bytes large.

   414

 *

   415

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

   416

 */

   417

int ec_datagram_lwr(

   418

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

   419

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

   420

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

   421

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

   422

        )

   423

{

   424

    datagram->data = external_memory;

   425

    datagram->data_origin = EC_ORIG_EXTERNAL;

   426

    EC_FUNC_HEADER;

   427

    datagram->type = EC_DATAGRAM_LWR;

   428

    EC_WRITE_U32(datagram->address, offset);

   429

    EC_FUNC_FOOTER;

   430

}

   431

   432

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

   433

   434

/** Initializes an EtherCAT LRW datagram.

   435

 *

   436

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

   437

 *            bytes large.

   438

 *

   439

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

   440

 */

   441

int ec_datagram_lrw(

   442

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

   443

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

   444

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

   445

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

   446

        )

   447

{

   448

    datagram->data = external_memory;

   449

    datagram->data_origin = EC_ORIG_EXTERNAL;

   450

    EC_FUNC_HEADER;

   451

    datagram->type = EC_DATAGRAM_LRW;

   452

    EC_WRITE_U32(datagram->address, offset);

   453

    EC_FUNC_FOOTER;

   454

}

   455

   456

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

   457

   458

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

   459

 *

   460

 * Outputs an error message.

   461

 */

   462

void ec_datagram_print_wc_error(

   463

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

   464

        )

   465

{

   466

    if (datagram->working_counter == 0)

   467

        printk("No response.");

   468

    else if (datagram->working_counter > 1)

   469

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

   470

    else

   471

        printk("Success.");

   472

    printk("\n");

   473

}

   474

   475

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

   476

   477

/** Outputs datagram statistics at most every second.

   478

 */

   479

void ec_datagram_output_stats(

   480

        ec_datagram_t *datagram

   481

        )

   482

{

   483

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

   484

        datagram->stats_output_jiffies = jiffies;

   485

   486

        if (unlikely(datagram->skip_count)) {

   487

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

   488

                    (unsigned int) datagram, datagram->name,

   489

                    datagram->skip_count,

   490

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

   491

            datagram->skip_count = 0;