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

#ifdef EC_HAVE_CYCLES

   105

    datagram->cycles_sent = 0;

   106

#endif

   107

    datagram->jiffies_sent = 0;

   108

#ifdef EC_HAVE_CYCLES

   109

    datagram->cycles_received = 0;

   110

#endif

   111

    datagram->jiffies_received = 0;

   112

    datagram->skip_count = 0;

   113

    datagram->stats_output_jiffies = 0;

   114

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

   115

}

   116

   117

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

   118

   119

/** Destructor.

   120

 */

   121

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

   122

{

   123

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

   124

        kfree(datagram->data);

   125

}

   126

   127

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

   128

   129

/** Allocates internal payload memory.

   130

 *

   131

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

   132

 *

   133

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

   134

 *            is done!

   135

 *

   136

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

   137

 */

   138

int ec_datagram_prealloc(

   139

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

   140

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

   141

        )

   142

{

   143

    if (datagram->data_origin == EC_ORIG_EXTERNAL

   144

            || size <= datagram->mem_size)

   145

        return 0;

   146

   147

    if (datagram->data) {

   148

        kfree(datagram->data);

   149

        datagram->data = NULL;

   150

        datagram->mem_size = 0;

   151

    }

   152

   153

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

   154

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

   155

        return -1;

   156

    }

   157

   158

    datagram->mem_size = size;

   159

    return 0;

   160

}

   161

   162

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

   163

   164

/** Initializes an EtherCAT APRD datagram.

   165

 *

   166

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

   167

 */

   168

int ec_datagram_aprd(

   169

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

   170

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

   171

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

   172

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

   173

        )

   174

{

   175

    EC_FUNC_HEADER;

   176

    datagram->type = EC_DATAGRAM_APRD;

   177

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

   178

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

   179

    EC_FUNC_FOOTER;

   180

}

   181

   182

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

   183

   184

/** Initializes an EtherCAT APWR datagram.

   185

 *

   186

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

   187

 */

   188

int ec_datagram_apwr(

   189

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

   190

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

   191

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

   192

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

   193

        )

   194

{

   195

    EC_FUNC_HEADER;

   196

    datagram->type = EC_DATAGRAM_APWR;

   197

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

   198

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

   199

    EC_FUNC_FOOTER;

   200

}

   201

   202

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

   203

   204

/** Initializes an EtherCAT APRW datagram.

   205

 *

   206

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

   207

 */

   208

int ec_datagram_aprw(

   209

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

   210

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

   211

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

   212

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

   213

        )

   214

{

   215

    EC_FUNC_HEADER;

   216

    datagram->type = EC_DATAGRAM_APRW;

   217

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

   218

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

   219

    EC_FUNC_FOOTER;

   220

}

   221

   222

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

   223

   224

/** Initializes an EtherCAT ARMW datagram.

   225

 *

   226

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

   227

 */

   228

int ec_datagram_armw(

   229

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

   230

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

   231

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

   232

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

   233

        )

   234

{

   235

    EC_FUNC_HEADER;

   236

    datagram->type = EC_DATAGRAM_ARMW;

   237

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

   238

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

   239

    EC_FUNC_FOOTER;

   240

}

   241

   242

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

   243

   244

/** Initializes an EtherCAT FPRD datagram.

   245

 *

   246

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

   247

 */

   248

int ec_datagram_fprd(

   249

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

   250

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

   251

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

   252

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

   253

        )

   254

{

   255

    if (unlikely(configured_address == 0x0000))

   256

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

   257

   258

    EC_FUNC_HEADER;

   259

    datagram->type = EC_DATAGRAM_FPRD;

   260

    EC_WRITE_U16(datagram->address, configured_address);

   261

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

   262

    EC_FUNC_FOOTER;

   263

}

   264

   265

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

   266

   267

/** Initializes an EtherCAT FPWR datagram.

   268

 *

   269

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

   270

 */

   271

int ec_datagram_fpwr(

   272

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

   273

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

   274

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

   275

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

   276

        )

   277

{

   278

    if (unlikely(configured_address == 0x0000))

   279

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

   280

   281

    EC_FUNC_HEADER;

   282

    datagram->type = EC_DATAGRAM_FPWR;

   283

    EC_WRITE_U16(datagram->address, configured_address);

   284

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

   285

    EC_FUNC_FOOTER;

   286

}

   287

   288

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

   289

   290

/** Initializes an EtherCAT FPRW datagram.

   291

 *

   292

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

   293

 */

   294

int ec_datagram_fprw(

   295

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

   296

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

   297

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

   298

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

   299

        )

   300

{

   301

    if (unlikely(configured_address == 0x0000))

   302

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

   303

   304

    EC_FUNC_HEADER;

   305

    datagram->type = EC_DATAGRAM_FPRW;

   306

    EC_WRITE_U16(datagram->address, configured_address);

   307

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

   308

    EC_FUNC_FOOTER;

   309

}

   310

   311

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

   312

   313

/** Initializes an EtherCAT FRMW datagram.

   314

 *

   315

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

   316

 */

   317

int ec_datagram_frmw(

   318

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

   319

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

   320

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

   321

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

   322

        )

   323

{

   324

    if (unlikely(configured_address == 0x0000))

   325

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

   326

   327

    EC_FUNC_HEADER;

   328

    datagram->type = EC_DATAGRAM_FRMW;

   329

    EC_WRITE_U16(datagram->address, configured_address);

   330

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

   331

    EC_FUNC_FOOTER;

   332

}

   333

   334

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

   335

   336

/** Initializes an EtherCAT BRD datagram.

   337

 *

   338

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

   339

 */

   340

int ec_datagram_brd(

   341

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

   342

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

   343

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

   344

        )

   345

{

   346

    EC_FUNC_HEADER;

   347

    datagram->type = EC_DATAGRAM_BRD;

   348

    EC_WRITE_U16(datagram->address, 0x0000);

   349

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

   350

    EC_FUNC_FOOTER;

   351

}

   352

   353

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

   354

   355

/** Initializes an EtherCAT BWR datagram.

   356

 *

   357

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

   358

 */

   359

int ec_datagram_bwr(

   360

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

   361

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

   362

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

   363

        )

   364

{

   365

    EC_FUNC_HEADER;

   366

    datagram->type = EC_DATAGRAM_BWR;

   367

    EC_WRITE_U16(datagram->address, 0x0000);

   368

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

   369

    EC_FUNC_FOOTER;

   370

}

   371

   372

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

   373

   374

/** Initializes an EtherCAT BRW datagram.

   375

 *

   376

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

   377

 */

   378

int ec_datagram_brw(

   379

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

   380

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

   381

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

   382

        )

   383

{

   384

    EC_FUNC_HEADER;

   385

    datagram->type = EC_DATAGRAM_BRW;

   386

    EC_WRITE_U16(datagram->address, 0x0000);

   387

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

   388

    EC_FUNC_FOOTER;

   389

}

   390

   391

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

   392

   393

/** Initializes an EtherCAT LRD datagram.

   394

 *

   395

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

   396

 *            bytes large.

   397

 *

   398

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

   399

 */

   400

int ec_datagram_lrd(

   401

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

   402

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

   403

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

   404

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

   405

        )

   406

{

   407

    datagram->data = external_memory;

   408

    datagram->data_origin = EC_ORIG_EXTERNAL;

   409

    EC_FUNC_HEADER;

   410

    datagram->type = EC_DATAGRAM_LRD;

   411

    EC_WRITE_U32(datagram->address, offset);

   412

    EC_FUNC_FOOTER;

   413

}

   414

   415

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

   416

   417

/** Initializes an EtherCAT LWR datagram.

   418

 *

   419

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

   420

 *            bytes large.

   421

 *

   422

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

   423

 */

   424

int ec_datagram_lwr(

   425

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

   426

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

   427

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

   428

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

   429

        )

   430

{

   431

    datagram->data = external_memory;

   432

    datagram->data_origin = EC_ORIG_EXTERNAL;

   433

    EC_FUNC_HEADER;

   434

    datagram->type = EC_DATAGRAM_LWR;

   435

    EC_WRITE_U32(datagram->address, offset);

   436

    EC_FUNC_FOOTER;

   437

}

   438

   439

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

   440

   441

/** Initializes an EtherCAT LRW datagram.

   442

 *

   443

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

   444

 *            bytes large.

   445

 *

   446

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

   447

 */

   448

int ec_datagram_lrw(

   449

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

   450

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

   451

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

   452

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

   453

        )

   454

{

   455

    datagram->data = external_memory;

   456

    datagram->data_origin = EC_ORIG_EXTERNAL;

   457

    EC_FUNC_HEADER;

   458

    datagram->type = EC_DATAGRAM_LRW;

   459

    EC_WRITE_U32(datagram->address, offset);

   460

    EC_FUNC_FOOTER;

   461

}

   462

   463

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

   464

   465

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

   466

 *

   467

 * Outputs an error message.

   468

 */

   469

void ec_datagram_print_wc_error(

   470

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

   471

        )

   472

{

   473

    if (datagram->working_counter == 0)

   474

        printk("No response.");

   475

    else if (datagram->working_counter > 1)

   476

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

   477

    else

   478

        printk("Success.");

   479

    printk("\n");

   480

}

   481

   482

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

   483

   484

/** Outputs datagram statistics at most every second.

   485

 */

   486

void ec_datagram_output_stats(

   487

        ec_datagram_t *datagram

   488

        )

   489

{

   490

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

   491

        datagram->stats_output_jiffies = jiffies;