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

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

    67

 *

    68

 * \attention This is indexed by ec_datagram_type_t.

    69

 */

    70

static const char *type_strings[] = {

    71

    "?",

    72

    "APRD",

    73

    "APWR",

    74

    "APRW",

    75

    "FPRD",

    76

    "FPWR",

    77

    "FPRW",

    78

    "BRD",

    79

    "BWR",

    80

    "BRW",

    81

    "LRD",

    82

    "LWR",

    83

    "LRW",

    84

    "ARMW",

    85

    "FRMW"

    86

};

    87

    88

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

    89

    90

/** Constructor.

    91

 */

    92

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

    93

{

    94

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

    95

    datagram->type = EC_DATAGRAM_NONE;

    96

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

    97

    datagram->data = NULL;

    98

    datagram->data_origin = EC_ORIG_INTERNAL;

    99

    datagram->mem_size = 0;

   100

    datagram->data_size = 0;

   101

    datagram->index = 0x00;

   102

    datagram->working_counter = 0x0000;

   103

    datagram->state = EC_DATAGRAM_INIT;

   104

    datagram->cycles_sent = 0;

   105

    datagram->jiffies_sent = 0;

   106

    datagram->cycles_received = 0;

   107

    datagram->jiffies_received = 0;

   108

    datagram->skip_count = 0;

   109

    datagram->stats_output_jiffies = 0;

   110

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

   111

}

   112

   113

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

   114

   115

/** Destructor.

   116

 */

   117

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

   118

{

   119

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

   120

        kfree(datagram->data);

   121

}

   122

   123

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

   124

   125

/** Allocates internal payload memory.

   126

 *

   127

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

   128

 *

   129

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

   130

 *            is done!

   131

 *

   132

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

   133

 */

   134

int ec_datagram_prealloc(

   135

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

   136

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

   137

        )

   138

{

   139

    if (datagram->data_origin == EC_ORIG_EXTERNAL

   140

            || size <= datagram->mem_size)

   141

        return 0;

   142

   143

    if (datagram->data) {

   144

        kfree(datagram->data);

   145

        datagram->data = NULL;

   146

        datagram->mem_size = 0;

   147

    }

   148

   149

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

   150

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

   151

        return -1;

   152

    }

   153

   154

    datagram->mem_size = size;

   155

    return 0;

   156

}

   157

   158

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

   159

   160

/** Initializes an EtherCAT APRD datagram.

   161

 *

   162

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

   163

 */

   164

int ec_datagram_aprd(

   165

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

   166

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

   167

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

   168

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

   169

        )

   170

{

   171

    EC_FUNC_HEADER;

   172

    datagram->type = EC_DATAGRAM_APRD;

   173

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

   174

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

   175

    EC_FUNC_FOOTER;

   176

}

   177

   178

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

   179

   180

/** Initializes an EtherCAT APWR datagram.

   181

 *

   182

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

   183

 */

   184

int ec_datagram_apwr(

   185

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

   186

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

   187

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

   188

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

   189

        )

   190

{

   191

    EC_FUNC_HEADER;

   192

    datagram->type = EC_DATAGRAM_APWR;

   193

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

   194

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

   195

    EC_FUNC_FOOTER;

   196

}

   197

   198

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

   199

   200

/** Initializes an EtherCAT APRW datagram.

   201

 *

   202

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

   203

 */

   204

int ec_datagram_aprw(

   205

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

   206

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

   207

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

   208

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

   209

        )

   210

{

   211

    EC_FUNC_HEADER;

   212

    datagram->type = EC_DATAGRAM_APRW;

   213

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

   214

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

   215

    EC_FUNC_FOOTER;

   216

}

   217

   218

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

   219

   220

/** Initializes an EtherCAT ARMW datagram.

   221

 *

   222

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

   223

 */

   224

int ec_datagram_armw(

   225

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

   226

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

   227

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

   228

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

   229

        )

   230

{

   231

    EC_FUNC_HEADER;

   232

    datagram->type = EC_DATAGRAM_ARMW;

   233

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

   234

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

   235

    EC_FUNC_FOOTER;

   236

}

   237

   238

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

   239

   240

/** Initializes an EtherCAT FPRD datagram.

   241

 *

   242

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

   243

 */

   244

int ec_datagram_fprd(

   245

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

   246

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

   247

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

   248

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

   249

        )

   250

{

   251

    if (unlikely(configured_address == 0x0000))

   252

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

   253

   254

    EC_FUNC_HEADER;

   255

    datagram->type = EC_DATAGRAM_FPRD;

   256

    EC_WRITE_U16(datagram->address, configured_address);

   257

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

   258

    EC_FUNC_FOOTER;

   259

}

   260

   261

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

   262

   263

/** Initializes an EtherCAT FPWR datagram.

   264

 *

   265

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

   266

 */

   267

int ec_datagram_fpwr(

   268

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

   269

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

   270

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

   271

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

   272

        )

   273

{

   274

    if (unlikely(configured_address == 0x0000))

   275

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

   276

   277

    EC_FUNC_HEADER;

   278

    datagram->type = EC_DATAGRAM_FPWR;

   279

    EC_WRITE_U16(datagram->address, configured_address);

   280

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

   281

    EC_FUNC_FOOTER;

   282

}

   283

   284

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

   285

   286

/** Initializes an EtherCAT FPRW datagram.

   287

 *

   288

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

   289

 */

   290

int ec_datagram_fprw(

   291

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

   292

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

   293

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

   294

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

   295

        )

   296

{

   297

    if (unlikely(configured_address == 0x0000))

   298

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

   299

   300

    EC_FUNC_HEADER;

   301

    datagram->type = EC_DATAGRAM_FPRW;

   302

    EC_WRITE_U16(datagram->address, configured_address);

   303

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

   304

    EC_FUNC_FOOTER;

   305

}

   306

   307

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

   308

   309

/** Initializes an EtherCAT FRMW datagram.

   310

 *

   311

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

   312

 */

   313

int ec_datagram_frmw(

   314

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

   315

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

   316

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

   317

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

   318

        )

   319

{

   320

    if (unlikely(configured_address == 0x0000))

   321

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

   322

   323

    EC_FUNC_HEADER;

   324

    datagram->type = EC_DATAGRAM_FRMW;

   325

    EC_WRITE_U16(datagram->address, configured_address);

   326

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

   327

    EC_FUNC_FOOTER;

   328

}

   329

   330

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

   331

   332

/** Initializes an EtherCAT BRD datagram.

   333

 *

   334

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

   335

 */

   336

int ec_datagram_brd(

   337

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

   338

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

   339

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

   340

        )

   341

{

   342

    EC_FUNC_HEADER;

   343

    datagram->type = EC_DATAGRAM_BRD;

   344

    EC_WRITE_U16(datagram->address, 0x0000);

   345

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

   346

    EC_FUNC_FOOTER;

   347

}

   348

   349

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

   350

   351

/** Initializes an EtherCAT BWR datagram.

   352

 *

   353

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

   354

 */

   355

int ec_datagram_bwr(

   356

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

   357

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

   358

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

   359

        )

   360

{

   361

    EC_FUNC_HEADER;

   362

    datagram->type = EC_DATAGRAM_BWR;

   363

    EC_WRITE_U16(datagram->address, 0x0000);

   364

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

   365

    EC_FUNC_FOOTER;

   366

}

   367

   368

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

   369

   370

/** Initializes an EtherCAT BRW datagram.

   371

 *

   372

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

   373

 */

   374

int ec_datagram_brw(

   375

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

   376

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

   377

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

   378

        )

   379

{

   380

    EC_FUNC_HEADER;

   381

    datagram->type = EC_DATAGRAM_BRW;

   382

    EC_WRITE_U16(datagram->address, 0x0000);

   383

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

   384

    EC_FUNC_FOOTER;

   385

}

   386

   387

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

   388

   389

/** Initializes an EtherCAT LRD datagram.

   390

 *

   391

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

   392

 *            bytes large.

   393

 *

   394

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

   395

 */

   396

int ec_datagram_lrd(

   397

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

   398

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

   399

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

   400

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

   401

        )

   402

{

   403

    datagram->data = external_memory;

   404

    datagram->data_origin = EC_ORIG_EXTERNAL;

   405

    EC_FUNC_HEADER;

   406

    datagram->type = EC_DATAGRAM_LRD;

   407

    EC_WRITE_U32(datagram->address, offset);

   408

    EC_FUNC_FOOTER;

   409

}

   410

   411

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

   412

   413

/** Initializes an EtherCAT LWR datagram.

   414

 *

   415

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

   416

 *            bytes large.

   417

 *

   418

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

   419

 */

   420

int ec_datagram_lwr(

   421

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

   422

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

   423

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

   424

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

   425

        )

   426

{

   427

    datagram->data = external_memory;

   428

    datagram->data_origin = EC_ORIG_EXTERNAL;

   429

    EC_FUNC_HEADER;

   430

    datagram->type = EC_DATAGRAM_LWR;

   431

    EC_WRITE_U32(datagram->address, offset);

   432

    EC_FUNC_FOOTER;

   433

}

   434

   435

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

   436

   437

/** Initializes an EtherCAT LRW datagram.

   438

 *

   439

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

   440

 *            bytes large.

   441

 *

   442

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

   443

 */

   444

int ec_datagram_lrw(

   445

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

   446

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

   447

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

   448

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

   449

        )

   450

{

   451

    datagram->data = external_memory;

   452

    datagram->data_origin = EC_ORIG_EXTERNAL;

   453

    EC_FUNC_HEADER;

   454

    datagram->type = EC_DATAGRAM_LRW;

   455

    EC_WRITE_U32(datagram->address, offset);

   456

    EC_FUNC_FOOTER;

   457

}

   458

   459

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

   460

   461

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

   462

 *

   463

 * Outputs an error message.

   464

 */

   465

void ec_datagram_print_wc_error(

   466

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

   467

        )

   468

{

   469

    if (datagram->working_counter == 0)

   470

        printk("No response.");

   471

    else if (datagram->working_counter > 1)

   472

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

   473

    else

   474

        printk("Success.");

   475

    printk("\n");

   476

}

   477

   478

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

   479

   480

/** Outputs datagram statistics at most every second.

   481

 */

   482

void ec_datagram_output_stats(

   483

        ec_datagram_t *datagram

   484

        )

   485

{

   486

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

   487

        datagram->stats_output_jiffies = jiffies;

   488

   489

        if (unlikely(datagram->skip_count)) {

   490

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

   491

                    (unsigned int) datagram, datagram->name,

   492