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

    ret = ec_datagram_prealloc(datagram, data_size); \

    52

    if (unlikely(ret)) \

    53

        return ret; \

    54

    datagram->index = 0; \

    55

    datagram->working_counter = 0; \

    56

    datagram->state = EC_DATAGRAM_INIT;

    57

    58

#define EC_FUNC_FOOTER \

    59

    datagram->data_size = 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, otherwise \a -ENOMEM.

   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 -ENOMEM;

   156

    }

   157

   158

    datagram->mem_size = size;

   159

    return 0;

   160

}

   161

   162

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

   163

   164

/** Fills the datagram payload memory with zeros.

   165

 */

   166

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

   167

{

   168

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

   169

}

   170

   171

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

   172

   173

/** Initializes an EtherCAT APRD datagram.

   174

 *

   175

 * \return Return value of ec_datagram_prealloc().

   176

 */

   177

int ec_datagram_aprd(

   178

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

   179

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

   180

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

   181

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

   182

        )

   183

{

   184

    int ret;

   185

    EC_FUNC_HEADER;

   186

    datagram->type = EC_DATAGRAM_APRD;

   187

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

   188

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

   189

    EC_FUNC_FOOTER;

   190

}

   191

   192

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

   193

   194

/** Initializes an EtherCAT APWR datagram.

   195

 *

   196

 * \return Return value of ec_datagram_prealloc().

   197

 */

   198

int ec_datagram_apwr(

   199

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

   200

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

   201

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

   202

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

   203

        )

   204

{

   205

    int ret;

   206

    EC_FUNC_HEADER;

   207

    datagram->type = EC_DATAGRAM_APWR;

   208

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

   209

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

   210

    EC_FUNC_FOOTER;

   211

}

   212

   213

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

   214

   215

/** Initializes an EtherCAT APRW datagram.

   216

 *

   217

 * \return Return value of ec_datagram_prealloc().

   218

 */

   219

int ec_datagram_aprw(

   220

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

   221

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

   222

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

   223

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

   224

        )

   225

{

   226

    int ret;

   227

    EC_FUNC_HEADER;

   228

    datagram->type = EC_DATAGRAM_APRW;

   229

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

   230

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

   231

    EC_FUNC_FOOTER;

   232

}

   233

   234

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

   235

   236

/** Initializes an EtherCAT ARMW datagram.

   237

 *

   238

 * \return Return value of ec_datagram_prealloc().

   239

 */

   240

int ec_datagram_armw(

   241

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

   242

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

   243

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

   244

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

   245

        )

   246

{

   247

    int ret;

   248

    EC_FUNC_HEADER;

   249

    datagram->type = EC_DATAGRAM_ARMW;

   250

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

   251

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

   252

    EC_FUNC_FOOTER;

   253

}

   254

   255

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

   256

   257

/** Initializes an EtherCAT FPRD datagram.

   258

 *

   259

 * \return Return value of ec_datagram_prealloc().

   260

 */

   261

int ec_datagram_fprd(

   262

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

   263

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

   264

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

   265

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

   266

        )

   267

{

   268

    int ret;

   269

   270

    if (unlikely(configured_address == 0x0000))

   271

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

   272

   273

    EC_FUNC_HEADER;

   274

    datagram->type = EC_DATAGRAM_FPRD;

   275

    EC_WRITE_U16(datagram->address, configured_address);

   276

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

   277

    EC_FUNC_FOOTER;

   278

}

   279

   280

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

   281

   282

/** Initializes an EtherCAT FPWR datagram.

   283

 *

   284

 * \return Return value of ec_datagram_prealloc().

   285

 */

   286

int ec_datagram_fpwr(

   287

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

   288

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

   289

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

   290

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

   291

        )

   292

{

   293

    int ret;

   294

   295

    if (unlikely(configured_address == 0x0000))

   296

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

   297

   298

    EC_FUNC_HEADER;

   299

    datagram->type = EC_DATAGRAM_FPWR;

   300

    EC_WRITE_U16(datagram->address, configured_address);

   301

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

   302

    EC_FUNC_FOOTER;

   303

}

   304

   305

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

   306

   307

/** Initializes an EtherCAT FPRW datagram.

   308

 *

   309

 * \return Return value of ec_datagram_prealloc().

   310

 */

   311

int ec_datagram_fprw(

   312

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

   313

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

   314

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

   315

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

   316

        )

   317

{

   318

    int ret;

   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_FPRW;

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

   333

 *

   334

 * \return Return value of ec_datagram_prealloc().

   335

 */

   336

int ec_datagram_frmw(

   337

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

   338

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

   339

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

   340

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

   341

        )

   342

{

   343

    int ret;

   344

   345

    if (unlikely(configured_address == 0x0000))

   346

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

   347

   348

    EC_FUNC_HEADER;

   349

    datagram->type = EC_DATAGRAM_FRMW;

   350

    EC_WRITE_U16(datagram->address, configured_address);

   351

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

   352

    EC_FUNC_FOOTER;

   353

}

   354

   355

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

   356

   357

/** Initializes an EtherCAT BRD datagram.

   358

 *

   359

 * \return Return value of ec_datagram_prealloc().

   360

 */

   361

int ec_datagram_brd(

   362

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

   363

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

   364

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

   365

        )

   366

{

   367

    int ret;

   368

    EC_FUNC_HEADER;

   369

    datagram->type = EC_DATAGRAM_BRD;

   370

    EC_WRITE_U16(datagram->address, 0x0000);

   371

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

   372

    EC_FUNC_FOOTER;

   373

}

   374

   375

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

   376

   377

/** Initializes an EtherCAT BWR datagram.

   378

 *

   379

 * \return Return value of ec_datagram_prealloc().

   380

 */

   381

int ec_datagram_bwr(

   382

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

   383

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

   384

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

   385

        )

   386

{

   387

    int ret;

   388

    EC_FUNC_HEADER;

   389

    datagram->type = EC_DATAGRAM_BWR;

   390

    EC_WRITE_U16(datagram->address, 0x0000);

   391

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

   392

    EC_FUNC_FOOTER;

   393

}

   394

   395

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

   396

   397

/** Initializes an EtherCAT BRW datagram.

   398

 *

   399

 * \return Return value of ec_datagram_prealloc().

   400

 */

   401

int ec_datagram_brw(

   402

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

   403

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

   404

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

   405

        )

   406

{

   407

    int ret;

   408

    EC_FUNC_HEADER;

   409

    datagram->type = EC_DATAGRAM_BRW;

   410

    EC_WRITE_U16(datagram->address, 0x0000);

   411

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

   412

    EC_FUNC_FOOTER;

   413

}

   414

   415

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

   416

   417

/** Initializes an EtherCAT LRD datagram.

   418

 *

   419

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

   420

 *            bytes large.

   421

 *

   422

 * \return Return value of ec_datagram_prealloc().

   423

 */

   424

int ec_datagram_lrd(

   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

    int ret;

   432

    datagram->data = external_memory;

   433

    datagram->data_origin = EC_ORIG_EXTERNAL;

   434

    EC_FUNC_HEADER;

   435

    datagram->type = EC_DATAGRAM_LRD;

   436

    EC_WRITE_U32(datagram->address, offset);

   437

    EC_FUNC_FOOTER;

   438

}

   439

   440

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

   441

   442

/** Initializes an EtherCAT LWR datagram.

   443

 *

   444

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

   445

 *            bytes large.

   446

 *

   447

 * \return Return value of ec_datagram_prealloc().

   448

 */

   449

int ec_datagram_lwr(

   450

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

   451

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

   452

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

   453

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

   454

        )

   455

{

   456

    int ret;

   457

    datagram->data = external_memory;

   458

    datagram->data_origin = EC_ORIG_EXTERNAL;

   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

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

   470

 *            bytes large.

   471

 *

   472

 * \return Return value of ec_datagram_prealloc().

   473

 */

   474

int ec_datagram_lrw(

   475

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

   476

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

   477

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

   478

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

   479

        )

   480

{

   481

    int ret;

   482

    datagram->data = external_memory;

   483

    datagram->data_origin = EC_ORIG_EXTERNAL;

   484

    EC_FUNC_HEADER;

   485

    datagram->type = EC_DATAGRAM_LRW;

   486

    EC_WRITE_U32(datagram->address, offset);

   487

    EC_FUNC_FOOTER;

   488

}