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

    	datagram->data = NULL;

   119

    }

   120

}

   121

   122

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

   123

   124

/** Allocates internal payload memory.

   125

 *

   126

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

   127

 *

   128

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

   129

 *            is done!

   130

 *

   131

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

   132

 */

   133

int ec_datagram_prealloc(

   134

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

   135

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

   136

        )

   137

{

   138

    if (datagram->data_origin == EC_ORIG_EXTERNAL

   139

            || size <= datagram->mem_size)

   140

        return 0;

   141

   142

    if (datagram->data) {

   143

        kfree(datagram->data);

   144

        datagram->data = NULL;

   145

        datagram->mem_size = 0;

   146

    }

   147

   148

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

   149

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

   150

        return -1;

   151

    }

   152

   153

    datagram->mem_size = size;

   154

    return 0;

   155

}

   156

   157

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

   158

   159

/** Initializes an EtherCAT APRD datagram.

   160

 *

   161

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

   162

 */

   163

int ec_datagram_aprd(

   164

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

   165

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

   166

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

   167

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

   168

        )

   169

{

   170

    EC_FUNC_HEADER;

   171

    datagram->type = EC_DATAGRAM_APRD;

   172

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

   173

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

   174

    EC_FUNC_FOOTER;

   175

}

   176

   177

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

   178

   179

/** Initializes an EtherCAT APWR datagram.

   180

 *

   181

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

   182

 */

   183

int ec_datagram_apwr(

   184

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

   185

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

   186

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

   187

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

   188

        )

   189

{

   190

    EC_FUNC_HEADER;

   191

    datagram->type = EC_DATAGRAM_APWR;

   192

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

   193

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

   194

    EC_FUNC_FOOTER;

   195

}

   196

   197

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

   198

   199

/** Initializes an EtherCAT APRW datagram.

   200

 *

   201

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

   202

 */

   203

int ec_datagram_aprw(

   204

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

   205

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

   206

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

   207

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

   208

        )

   209

{

   210

    EC_FUNC_HEADER;

   211

    datagram->type = EC_DATAGRAM_APRW;

   212

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

   213

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

   214

    EC_FUNC_FOOTER;

   215

}

   216

   217

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

   218

   219

/** Initializes an EtherCAT ARMW datagram.

   220

 *

   221

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

   222

 */

   223

int ec_datagram_armw(

   224

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

   225

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

   226

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

   227

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

   228

        )

   229

{

   230

    EC_FUNC_HEADER;

   231

    datagram->type = EC_DATAGRAM_ARMW;

   232

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

   233

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

   234

    EC_FUNC_FOOTER;

   235

}

   236

   237

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

   238

   239

/** Initializes an EtherCAT FPRD datagram.

   240

 *

   241

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

   242

 */

   243

int ec_datagram_fprd(

   244

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

   245

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

   246

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

   247

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

   248

        )

   249

{

   250

    if (unlikely(configured_address == 0x0000))

   251

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

   252

   253

    EC_FUNC_HEADER;

   254

    datagram->type = EC_DATAGRAM_FPRD;

   255

    EC_WRITE_U16(datagram->address, configured_address);

   256

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

   257

    EC_FUNC_FOOTER;

   258

}

   259

   260

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

   261

   262

/** Initializes an EtherCAT FPWR datagram.

   263

 *

   264

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

   265

 */

   266

int ec_datagram_fpwr(

   267

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

   268

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

   269

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

   270

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

   271

        )

   272

{

   273

    if (unlikely(configured_address == 0x0000))

   274

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

   275

   276

    EC_FUNC_HEADER;

   277

    datagram->type = EC_DATAGRAM_FPWR;

   278

    EC_WRITE_U16(datagram->address, configured_address);

   279

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

   280

    EC_FUNC_FOOTER;

   281

}

   282

   283

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

   284

   285

/** Initializes an EtherCAT FPRW datagram.

   286

 *

   287

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

   288

 */

   289

int ec_datagram_fprw(

   290

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

   291

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

   292

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

   293

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

   294

        )

   295

{

   296

    if (unlikely(configured_address == 0x0000))

   297

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

   298

   299

    EC_FUNC_HEADER;

   300

    datagram->type = EC_DATAGRAM_FPRW;

   301

    EC_WRITE_U16(datagram->address, configured_address);

   302

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

   303

    EC_FUNC_FOOTER;

   304

}

   305

   306

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

   307

   308

/** Initializes an EtherCAT FRMW datagram.

   309

 *

   310

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

   311

 */

   312

int ec_datagram_frmw(

   313

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

   314

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

   315

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

   316

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

   317

        )

   318

{

   319

    if (unlikely(configured_address == 0x0000))

   320

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

   321

   322

    EC_FUNC_HEADER;

   323

    datagram->type = EC_DATAGRAM_FRMW;

   324

    EC_WRITE_U16(datagram->address, configured_address);

   325

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

   326

    EC_FUNC_FOOTER;

   327

}

   328

   329

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

   330

   331

/** Initializes an EtherCAT BRD datagram.

   332

 *

   333

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

   334

 */

   335

int ec_datagram_brd(

   336

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

   337

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

   338

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

   339

        )

   340

{

   341

    EC_FUNC_HEADER;

   342

    datagram->type = EC_DATAGRAM_BRD;

   343

    EC_WRITE_U16(datagram->address, 0x0000);

   344

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

   345

    EC_FUNC_FOOTER;

   346

}

   347

   348

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

   349

   350

/** Initializes an EtherCAT BWR datagram.

   351

 *

   352

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

   353

 */

   354

int ec_datagram_bwr(

   355

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

   356

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

   357

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

   358

        )

   359

{

   360

    EC_FUNC_HEADER;

   361

    datagram->type = EC_DATAGRAM_BWR;

   362

    EC_WRITE_U16(datagram->address, 0x0000);

   363

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

   364

    EC_FUNC_FOOTER;

   365

}

   366

   367

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

   368

   369

/** Initializes an EtherCAT BRW datagram.

   370

 *

   371

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

   372

 */

   373

int ec_datagram_brw(

   374

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

   375

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

   376

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

   377

        )

   378

{

   379

    EC_FUNC_HEADER;

   380

    datagram->type = EC_DATAGRAM_BRW;

   381

    EC_WRITE_U16(datagram->address, 0x0000);

   382

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

   383

    EC_FUNC_FOOTER;

   384

}

   385

   386

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

   387

   388

/** Initializes an EtherCAT LRD datagram.

   389

 *

   390

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

   391

 *            bytes large.

   392

 *

   393

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

   394

 */

   395

int ec_datagram_lrd(

   396

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

   397

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

   398

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

   399

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

   400

        )

   401

{

   402

    datagram->data = external_memory;

   403

    datagram->data_origin = EC_ORIG_EXTERNAL;

   404

    EC_FUNC_HEADER;

   405

    datagram->type = EC_DATAGRAM_LRD;

   406

    EC_WRITE_U32(datagram->address, offset);

   407

    EC_FUNC_FOOTER;

   408

}

   409

   410

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

   411

   412

/** Initializes an EtherCAT LWR datagram.

   413

 *

   414

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

   415

 *            bytes large.

   416

 *

   417

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

   418

 */

   419

int ec_datagram_lwr(

   420

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

   421

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

   422

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

   423

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

   424

        )

   425

{

   426

    datagram->data = external_memory;

   427

    datagram->data_origin = EC_ORIG_EXTERNAL;

   428

    EC_FUNC_HEADER;

   429

    datagram->type = EC_DATAGRAM_LWR;

   430

    EC_WRITE_U32(datagram->address, offset);

   431

    EC_FUNC_FOOTER;

   432

}

   433

   434

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

   435

   436

/** Initializes an EtherCAT LRW datagram.

   437

 *

   438

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

   439

 *            bytes large.

   440

 *

   441

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

   442

 */

   443

int ec_datagram_lrw(

   444

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

   445

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

   446

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

   447

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

   448

        )

   449

{

   450

    datagram->data = external_memory;

   451

    datagram->data_origin = EC_ORIG_EXTERNAL;

   452

    EC_FUNC_HEADER;

   453

    datagram->type = EC_DATAGRAM_LRW;

   454

    EC_WRITE_U32(datagram->address, offset);

   455

    EC_FUNC_FOOTER;

   456

}

   457

   458

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

   459

   460

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

   461

 *

   462

 * Outputs an error message.

   463

 */

   464

void ec_datagram_print_wc_error(

   465

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

   466

        )

   467

{

   468

    if (datagram->working_counter == 0)

   469

        printk("No response.");

   470

    else if (datagram->working_counter > 1)

   471

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

   472

    else

   473

        printk("Success.");

   474

    printk("\n");

   475

}

   476

   477

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

   478

   479

/** Outputs datagram statistics at most every second.

   480

 */

   481

void ec_datagram_output_stats(

   482

        ec_datagram_t *datagram

   483

        )

   484

{

   485

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

   486

        datagram->stats_output_jiffies = jiffies;

   487

   488

        if (unlikely(datagram->skip_count)) {

   489

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

   490

                    (unsigned int) datagram, datagram->name,

   491

                    datagram->skip_count,