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

 *  ---

    23

 *

    24

 *  The license mentioned above concerns the source code only. Using the

    25

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

    26

 *  industrial property and similar rights of Beckhoff Automation GmbH.

    27

 *

    28

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

    29

    30

/**

    31

   \file

    32

   Methods of an EtherCAT datagram.

    33

*/

    34

    35

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

    36

    37

#include <linux/slab.h>

    38

    39

#include "datagram.h"

    40

#include "master.h"

    41

    42

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

    43

    44

/** \cond */

    45

    46

#define EC_FUNC_HEADER \

    47

    ret = ec_datagram_prealloc(datagram, data_size); \

    48

    if (unlikely(ret)) \

    49

        return ret; \

    50

    datagram->index = 0; \

    51

    datagram->working_counter = 0; \

    52

    datagram->state = EC_DATAGRAM_INIT;

    53

    54

#define EC_FUNC_FOOTER \

    55

    datagram->data_size = data_size; \

    56

    return 0;

    57

    58

/** \endcond */

    59

    60

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

    61

    62

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

    63

 *

    64

 * \attention This is indexed by ec_datagram_type_t.

    65

 */

    66

static const char *type_strings[] = {

    67

    "?",

    68

    "APRD",

    69

    "APWR",

    70

    "APRW",

    71

    "FPRD",

    72

    "FPWR",

    73

    "FPRW",

    74

    "BRD",

    75

    "BWR",

    76

    "BRW",

    77

    "LRD",

    78

    "LWR",

    79

    "LRW",

    80

    "ARMW",

    81

    "FRMW"

    82

};

    83

    84

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

    85

    86

/** Constructor.

    87

 */

    88

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

    89

{

    90

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

    91

    datagram->device_index = EC_DEVICE_MAIN;

    92

    datagram->type = EC_DATAGRAM_NONE;

    93

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

    94

    datagram->data = NULL;

    95

    datagram->data_origin = EC_ORIG_INTERNAL;

    96

    datagram->mem_size = 0;

    97

    datagram->data_size = 0;

    98

    datagram->index = 0x00;

    99

    datagram->working_counter = 0x0000;

   100

    datagram->state = EC_DATAGRAM_INIT;

   101

#ifdef EC_HAVE_CYCLES

   102

    datagram->cycles_sent = 0;

   103

#endif

   104

    datagram->jiffies_sent = 0;

   105

#ifdef EC_HAVE_CYCLES

   106

    datagram->cycles_received = 0;

   107

#endif

   108

    datagram->jiffies_received = 0;

   109

    datagram->skip_count = 0;

   110

    datagram->stats_output_jiffies = 0;

   111

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

   112

}

   113

   114

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

   115

   116

/** Destructor.

   117

 */

   118

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

   119

{

   120

    ec_datagram_unqueue(datagram);

   121

   122

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

   123

        kfree(datagram->data);

   124

        datagram->data = NULL;

   125

    }

   126

}

   127

   128

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

   129

   130

/** Unqueue datagram.

   131

 */

   132

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

   133

{

   134

    if (!list_empty(&datagram->queue)) {

   135

        list_del_init(&datagram->queue);

   136

    }

   137

}

   138

   139

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

   140

   141

/** Allocates internal payload memory.

   142

 *

   143

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

   144

 *

   145

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

   146

 *            is done!

   147

 *

   148

 * \return 0 in case of success, otherwise \a -ENOMEM.

   149

 */

   150

int ec_datagram_prealloc(

   151

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

   152

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

   153

        )

   154

{

   155

    if (datagram->data_origin == EC_ORIG_EXTERNAL

   156

            || size <= datagram->mem_size)

   157

        return 0;

   158

   159

    if (datagram->data) {

   160

        kfree(datagram->data);

   161

        datagram->data = NULL;

   162

        datagram->mem_size = 0;

   163

    }

   164

   165

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

   166

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

   167

        return -ENOMEM;

   168

    }

   169

   170

    datagram->mem_size = size;

   171

    return 0;

   172

}

   173

   174

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

   175

   176

/** Fills the datagram payload memory with zeros.

   177

 */

   178

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

   179

{

   180

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

   181

}

   182

   183

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

   184

   185

/** Initializes an EtherCAT APRD datagram.

   186

 *

   187

 * \return Return value of ec_datagram_prealloc().

   188

 */

   189

int ec_datagram_aprd(

   190

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

   191

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

   192

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

   193

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

   194

        )

   195

{

   196

    int ret;

   197

    EC_FUNC_HEADER;

   198

    datagram->type = EC_DATAGRAM_APRD;

   199

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

   200

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

   201

    EC_FUNC_FOOTER;

   202

}

   203

   204

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

   205

   206

/** Initializes an EtherCAT APWR datagram.

   207

 *

   208

 * \return Return value of ec_datagram_prealloc().

   209

 */

   210

int ec_datagram_apwr(

   211

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

   212

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

   213

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

   214

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

   215

        )

   216

{

   217

    int ret;

   218

    EC_FUNC_HEADER;

   219

    datagram->type = EC_DATAGRAM_APWR;

   220

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

   221

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

   222

    EC_FUNC_FOOTER;

   223

}

   224

   225

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

   226

   227

/** Initializes an EtherCAT APRW datagram.

   228

 *

   229

 * \return Return value of ec_datagram_prealloc().

   230

 */

   231

int ec_datagram_aprw(

   232

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

   233

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

   234

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

   235

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

   236

        )

   237

{

   238

    int ret;

   239

    EC_FUNC_HEADER;

   240

    datagram->type = EC_DATAGRAM_APRW;

   241

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

   242

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

   243

    EC_FUNC_FOOTER;

   244

}

   245

   246

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

   247

   248

/** Initializes an EtherCAT ARMW datagram.

   249

 *

   250

 * \return Return value of ec_datagram_prealloc().

   251

 */

   252

int ec_datagram_armw(

   253

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

   254

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

   255

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

   256

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

   257

        )

   258

{

   259

    int ret;

   260

    EC_FUNC_HEADER;

   261

    datagram->type = EC_DATAGRAM_ARMW;

   262

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

   263

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

   264

    EC_FUNC_FOOTER;

   265

}

   266

   267

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

   268

   269

/** Initializes an EtherCAT FPRD datagram.

   270

 *

   271

 * \return Return value of ec_datagram_prealloc().

   272

 */

   273

int ec_datagram_fprd(

   274

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

   275

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

   276

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

   277

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

   278

        )

   279

{

   280

    int ret;

   281

   282

    if (unlikely(configured_address == 0x0000))

   283

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

   284

   285

    EC_FUNC_HEADER;

   286

    datagram->type = EC_DATAGRAM_FPRD;

   287

    EC_WRITE_U16(datagram->address, configured_address);

   288

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

   289

    EC_FUNC_FOOTER;

   290

}

   291

   292

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

   293

   294

/** Initializes an EtherCAT FPWR datagram.

   295

 *

   296

 * \return Return value of ec_datagram_prealloc().

   297

 */

   298

int ec_datagram_fpwr(

   299

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

   300

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

   301

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

   302

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

   303

        )

   304

{

   305

    int ret;

   306

   307

    if (unlikely(configured_address == 0x0000))

   308

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

   309

   310

    EC_FUNC_HEADER;

   311

    datagram->type = EC_DATAGRAM_FPWR;

   312

    EC_WRITE_U16(datagram->address, configured_address);

   313

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

   314

    EC_FUNC_FOOTER;

   315

}

   316

   317

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

   318

   319

/** Initializes an EtherCAT FPRW datagram.

   320

 *

   321

 * \return Return value of ec_datagram_prealloc().

   322

 */

   323

int ec_datagram_fprw(

   324

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

   325

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

   326

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

   327

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

   328

        )

   329

{

   330

    int ret;

   331

   332

    if (unlikely(configured_address == 0x0000))

   333

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

   334

   335

    EC_FUNC_HEADER;

   336

    datagram->type = EC_DATAGRAM_FPRW;

   337

    EC_WRITE_U16(datagram->address, configured_address);

   338

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

   339

    EC_FUNC_FOOTER;

   340

}

   341

   342

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

   343

   344

/** Initializes an EtherCAT FRMW datagram.

   345

 *

   346

 * \return Return value of ec_datagram_prealloc().

   347

 */

   348

int ec_datagram_frmw(

   349

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

   350

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

   351

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

   352

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

   353

        )

   354

{

   355

    int ret;

   356

   357

    if (unlikely(configured_address == 0x0000))

   358

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

   359

   360

    EC_FUNC_HEADER;

   361

    datagram->type = EC_DATAGRAM_FRMW;

   362

    EC_WRITE_U16(datagram->address, configured_address);

   363

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

   364

    EC_FUNC_FOOTER;

   365

}

   366

   367

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

   368

   369

/** Initializes an EtherCAT BRD datagram.

   370

 *

   371

 * \return Return value of ec_datagram_prealloc().

   372

 */

   373

int ec_datagram_brd(

   374

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

   375

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

   376

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

   377

        )

   378

{

   379

    int ret;

   380

    EC_FUNC_HEADER;

   381

    datagram->type = EC_DATAGRAM_BRD;

   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 BWR datagram.

   390

 *

   391

 * \return Return value of ec_datagram_prealloc().

   392

 */

   393

int ec_datagram_bwr(

   394

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

   395

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

   396

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

   397

        )

   398

{

   399

    int ret;

   400

    EC_FUNC_HEADER;

   401

    datagram->type = EC_DATAGRAM_BWR;

   402

    EC_WRITE_U16(datagram->address, 0x0000);

   403

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

   404

    EC_FUNC_FOOTER;

   405

}

   406

   407

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

   408

   409

/** Initializes an EtherCAT BRW datagram.

   410

 *

   411

 * \return Return value of ec_datagram_prealloc().

   412

 */

   413

int ec_datagram_brw(

   414

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

   415

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

   416

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

   417

        )

   418

{

   419

    int ret;

   420

    EC_FUNC_HEADER;

   421

    datagram->type = EC_DATAGRAM_BRW;

   422

    EC_WRITE_U16(datagram->address, 0x0000);

   423

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

   424

    EC_FUNC_FOOTER;

   425

}

   426

   427

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

   428

   429

/** Initializes an EtherCAT LRD datagram.

   430

 *

   431

 * \return Return value of ec_datagram_prealloc().

   432

 */

   433

int ec_datagram_lrd(

   434

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

   435

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

   436

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

   437

        )

   438

{

   439

    int ret;

   440

    EC_FUNC_HEADER;

   441

    datagram->type = EC_DATAGRAM_LRD;

   442

    EC_WRITE_U32(datagram->address, offset);

   443

    EC_FUNC_FOOTER;

   444

}

   445

   446

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

   447

   448

/** Initializes an EtherCAT LWR datagram.

   449

 *

   450

 * \return Return value of ec_datagram_prealloc().

   451

 */

   452

int ec_datagram_lwr(

   453

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

   454

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

   455

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

   456

        )

   457

{

   458

    int ret;

   459

    EC_FUNC_HEADER;

   460

    datagram->type = EC_DATAGRAM_LWR;

   461

    EC_WRITE_U32(datagram->address, offset);

   462

    EC_FUNC_FOOTER;

   463

}

   464

   465

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

   466

   467

/** Initializes an EtherCAT LRW datagram.

   468

 *

   469

 * \return Return value of ec_datagram_prealloc().

   470

 */

   471

int ec_datagram_lrw(

   472

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

   473

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

   474

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

   475

        )

   476

{

   477

    int ret;

   478

    EC_FUNC_HEADER;

   479

    datagram->type = EC_DATAGRAM_LRW;

   480

    EC_WRITE_U32(datagram->address, offset);

   481

    EC_FUNC_FOOTER;

   482

}

   483

   484

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

   485

   486

/** Initializes an EtherCAT LRD datagram with external memory.

   487

 *

   488

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

   489