1

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

     2

 *

     3

 *  f r a m e . c

     4

 *

     5

 *  Methoden für einen EtherCAT-Frame.

     6

 *

     7

 *  $Id$

     8

 *

     9

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

    10

    11

#include <linux/slab.h>

    12

#include <linux/delay.h>

    13

    14

#include "../include/EtherCAT_si.h"

    15

#include "frame.h"

    16

#include "master.h"

    17

    18

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

    19

    20

#define EC_FUNC_HEADER \

    21

    frame->master = master; \

    22

    frame->index = 0; \

    23

    frame->working_counter = 0;

    24

    25

#define EC_FUNC_WRITE_FOOTER \

    26

    frame->data_length = length; \

    27

    memcpy(frame->data, data, length);

    28

    29

#define EC_FUNC_READ_FOOTER \

    30

    frame->data_length = length; \

    31

    memset(frame->data, 0x00, length);

    32

    33

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

    34

    35

/**

    36

   Initialisiert ein EtherCAT-NPRD-Kommando.

    37

    38

   Node-adressed physical read.

    39

*/

    40

    41

void ec_frame_init_nprd(ec_frame_t *frame,

    42

                        /**< EtherCAT-Rahmen */

    43

                        ec_master_t *master,

    44

                        /**< EtherCAT-Master */

    45

                        uint16_t node_address,

    46

                        /**< Adresse des Knotens (Slaves) */

    47

                        uint16_t offset,

    48

                        /**< Physikalische Speicheradresse im Slave */

    49

                        unsigned int length

    50

                        /**< Länge der zu lesenden Daten */

    51

                        )

    52

{

    53

    if (unlikely(node_address == 0x0000))

    54

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

    55

    56

    EC_FUNC_HEADER;

    57

    58

    frame->type = ec_frame_type_nprd;

    59

    frame->address.physical.slave = node_address;

    60

    frame->address.physical.mem = offset;

    61

    62

    EC_FUNC_READ_FOOTER;

    63

}

    64

    65

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

    66

    67

/**

    68

   Initialisiert ein EtherCAT-NPWR-Kommando.

    69

    70

   Node-adressed physical write.

    71

*/

    72

    73

void ec_frame_init_npwr(ec_frame_t *frame,

    74

                        /**< EtherCAT-Rahmen */

    75

                        ec_master_t *master,

    76

                        /**< EtherCAT-Master */

    77

                        uint16_t node_address,

    78

                        /**< Adresse des Knotens (Slaves) */

    79

                        uint16_t offset,

    80

                        /**< Physikalische Speicheradresse im Slave */

    81

                        unsigned int length,

    82

                        /**< Länge der zu schreibenden Daten */

    83

                        const uint8_t *data

    84

                        /**< Zeiger auf Speicher mit zu schreibenden Daten */

    85

                        )

    86

{

    87

    if (unlikely(node_address == 0x0000))

    88

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

    89

    90

    EC_FUNC_HEADER;

    91

    92

    frame->type = ec_frame_type_npwr;

    93

    frame->address.physical.slave = node_address;

    94

    frame->address.physical.mem = offset;

    95

    96

    EC_FUNC_WRITE_FOOTER;

    97

}

    98

    99

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

   100

   101

/**

   102

   Initialisiert ein EtherCAT-APRD-Kommando.

   103

   104

   Autoincrement physical read.

   105

*/

   106

   107

void ec_frame_init_aprd(ec_frame_t *frame,

   108

                        /**< EtherCAT-Rahmen */

   109

                        ec_master_t *master,

   110

                        /**< EtherCAT-Master */

   111

                        uint16_t ring_position,

   112

                        /**< Position des Slaves im Bus */

   113

                        uint16_t offset,

   114

                        /**< Physikalische Speicheradresse im Slave */

   115

                        unsigned int length

   116

                        /**< Länge der zu lesenden Daten */

   117

                        )

   118

{

   119

    EC_FUNC_HEADER;

   120

   121

    frame->type = ec_frame_type_aprd;

   122

    frame->address.physical.slave = (int16_t) ring_position * (-1);

   123

    frame->address.physical.mem = offset;

   124

   125

    EC_FUNC_READ_FOOTER;

   126

}

   127

   128

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

   129

   130

/**

   131

   Initialisiert ein EtherCAT-APWR-Kommando.

   132

   133

   Autoincrement physical write.

   134

*/

   135

   136

void ec_frame_init_apwr(ec_frame_t *frame,

   137

                        /**< EtherCAT-Rahmen */

   138

                        ec_master_t *master,

   139

                        /**< EtherCAT-Master */

   140

                        uint16_t ring_position,

   141

                        /**< Position des Slaves im Bus */

   142

                        uint16_t offset,

   143

                        /**< Physikalische Speicheradresse im Slave */

   144

                        unsigned int length,

   145

                        /**< Länge der zu schreibenden Daten */

   146

                        const uint8_t *data

   147

                        /**< Zeiger auf Speicher mit zu schreibenden Daten */

   148

                        )

   149

{

   150

    EC_FUNC_HEADER;

   151

   152

    frame->type = ec_frame_type_apwr;

   153

    frame->address.physical.slave = (int16_t) ring_position * (-1);

   154

    frame->address.physical.mem = offset;

   155

   156

    EC_FUNC_WRITE_FOOTER;

   157

}

   158

   159

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

   160

   161

/**

   162

   Initialisiert ein EtherCAT-BRD-Kommando.

   163

   164

   Broadcast read.

   165

*/

   166

   167

void ec_frame_init_brd(ec_frame_t *frame,

   168

                       /**< EtherCAT-Rahmen */

   169

                       ec_master_t *master,

   170

                       /**< EtherCAT-Master */

   171

                       uint16_t offset,

   172

                       /**< Physikalische Speicheradresse im Slave */

   173

                       unsigned int length

   174

                       /**< Länge der zu lesenden Daten */

   175

                       )

   176

{

   177

    EC_FUNC_HEADER;

   178

   179

    frame->type = ec_frame_type_brd;

   180

    frame->address.physical.slave = 0x0000;

   181

    frame->address.physical.mem = offset;

   182

   183

    EC_FUNC_READ_FOOTER;

   184

}

   185

   186

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

   187

   188

/**

   189

   Initialisiert ein EtherCAT-BWR-Kommando.

   190

   191

   Broadcast write.

   192

*/

   193

   194

void ec_frame_init_bwr(ec_frame_t *frame,

   195

                       /**< EtherCAT-Rahmen */

   196

                       ec_master_t *master,

   197

                       /**< EtherCAT-Master */

   198

                       uint16_t offset,

   199

                       /**< Physikalische Speicheradresse im Slave */

   200

                       unsigned int length,

   201

                       /**< Länge der zu schreibenden Daten */

   202

                       const uint8_t *data

   203

                       /**< Zeiger auf Speicher mit zu schreibenden Daten */

   204

                       )

   205

{

   206

    EC_FUNC_HEADER;

   207

   208

    frame->type = ec_frame_type_bwr;

   209

    frame->address.physical.slave = 0x0000;

   210

    frame->address.physical.mem = offset;

   211

   212

    EC_FUNC_WRITE_FOOTER;

   213

}

   214

   215

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

   216

   217

/**

   218

   Initialisiert ein EtherCAT-LRW-Kommando.

   219

   220

   Logical read write.

   221

*/

   222

   223

void ec_frame_init_lrw(ec_frame_t *frame,

   224

                       /**< EtherCAT-Rahmen */

   225

                       ec_master_t *master,

   226

                       /**< EtherCAT-Master */

   227

                       uint32_t offset,

   228

                       /**< Logische Startadresse */

   229

                       unsigned int length,

   230

                       /**< Länge der zu lesenden/schreibenden Daten */

   231

                       uint8_t *data

   232

                       /**< Zeiger auf die Daten */

   233

                       )

   234

{

   235

    EC_FUNC_HEADER;

   236

   237

    frame->type = ec_frame_type_lrw;

   238

    frame->address.logical = offset;

   239

   240

    EC_FUNC_WRITE_FOOTER;

   241

}

   242

   243

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

   244

   245

/**

   246

   Sendet einen einzelnen EtherCAT-Rahmen.

   247

   248

   \return 0 bei Erfolg, sonst < 0

   249

*/

   250

   251

int ec_frame_send(ec_frame_t *frame /**< Rahmen zum Senden */)

   252

{

   253

    unsigned int command_size, frame_size, i;

   254

    uint8_t *data;

   255

   256

    if (unlikely(frame->master->debug_level > 0)) {

   257

        EC_DBG("ec_frame_send\n");

   258

    }

   259

   260

    command_size = frame->data_length + EC_COMMAND_HEADER_SIZE

   261

        + EC_COMMAND_FOOTER_SIZE;

   262

    frame_size = command_size + EC_FRAME_HEADER_SIZE;

   263

   264

    if (unlikely(frame_size > EC_MAX_FRAME_SIZE)) {

   265

        EC_ERR("Frame too long (%i)!\n", frame_size);

   266

        return -1;

   267

    }

   268

   269

    if (frame_size < EC_MIN_FRAME_SIZE) frame_size = EC_MIN_FRAME_SIZE;

   270

   271

    if (unlikely(frame->master->debug_level > 0)) {

   272

        EC_DBG("Frame length: %i\n", frame_size);

   273

    }

   274

   275

    frame->index = frame->master->command_index;

   276

    frame->master->command_index = (frame->master->command_index + 1) % 0x0100;

   277

   278

    if (unlikely(frame->master->debug_level > 0)) {

   279

        EC_DBG("Sending command index 0x%X\n", frame->index);

   280

    }

   281

   282

    // Zeiger auf Socket-Buffer holen

   283

    data = ec_device_prepare(&frame->master->device);

   284

   285

    // EtherCAT frame header

   286

    EC_WRITE_U16(data, (command_size & 0x7FF) | 0x1000);

   287

    data += EC_FRAME_HEADER_SIZE;

   288

   289

    // EtherCAT command header

   290

    EC_WRITE_U8 (data,     frame->type);

   291

    EC_WRITE_U8 (data + 1, frame->index);

   292

    EC_WRITE_U32(data + 2, frame->address.logical);

   293

    EC_WRITE_U16(data + 6, frame->data_length & 0x7FF);

   294

    EC_WRITE_U16(data + 8, 0x0000);

   295

    data += EC_COMMAND_HEADER_SIZE;

   296

   297

    // EtherCAT command data

   298

    memcpy(data, frame->data, frame->data_length);

   299

    data += frame->data_length;

   300

   301

    // EtherCAT command footer

   302

    EC_WRITE_U16(data, frame->working_counter);

   303

    data += EC_COMMAND_FOOTER_SIZE;

   304

   305

    // Pad with zeros

   306

    for (i = EC_FRAME_HEADER_SIZE + EC_COMMAND_HEADER_SIZE

   307

             + frame->data_length + EC_COMMAND_FOOTER_SIZE;

   308

         i < EC_MIN_FRAME_SIZE; i++)

   309

        EC_WRITE_U8(data++, 0x00);

   310

   311

    // Send frame

   312

    ec_device_send(&frame->master->device, frame_size);

   313

   314

    return 0;

   315

}

   316

   317

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

   318

   319

/**

   321

   322

   \return 0 bei Erfolg, sonst < 0

   323

*/

   324

   325

int ec_frame_receive(ec_frame_t *frame /**< Gesendeter Rahmen */)

   326

{

   327

    unsigned int received_length, frame_length, data_length;

   328

    uint8_t *data;

   329

    uint8_t command_type, command_index;

   330

    ec_device_t *device;

   331

   332

    device = &frame->master->device;

   333

   334

    if (!(received_length = ec_device_received(device))) return -1;

   335

   336

    device->state = EC_DEVICE_STATE_READY;

   337

   338

    if (unlikely(received_length < EC_FRAME_HEADER_SIZE)) {

   339

        EC_ERR("Received frame with incomplete EtherCAT frame header!\n");

   340

        ec_device_debug(device);

   341

        return -1;

   342

    }

   343

   344

    data = ec_device_data(device);

   345

   346

    // Länge des gesamten Frames prüfen

   347

    frame_length = EC_READ_U16(data) & 0x07FF;

   348

    data += EC_FRAME_HEADER_SIZE;

   349

   350

    if (unlikely(frame_length > received_length)) {

   351

        EC_ERR("Received corrupted frame (length does not match)!\n");

   352

        ec_device_debug(device);

   353

        return -1;

   354

    }

   355

   356

    // Command header

   357

    command_type =  EC_READ_U8(data);

   358

    command_index = EC_READ_U8(data + 1);

   359

    data_length =   EC_READ_U16(data + 6) & 0x07FF;

   360

    data += EC_COMMAND_HEADER_SIZE;

   361

   362

    if (unlikely(EC_FRAME_HEADER_SIZE + EC_COMMAND_HEADER_SIZE

   363

                 + data_length + EC_COMMAND_FOOTER_SIZE > received_length)) {

   364

        EC_ERR("Received frame with incomplete command data!\n");

   365

        ec_device_debug(device);

   366

        return -1;

   367

    }

   368

   369

    if (unlikely(frame->type != command_type

   370

                 || frame->index != command_index

   371

                 || frame->data_length != data_length))

   372

    {

   373

        EC_WARN("WARNING - Send/Receive anomaly!\n");

   374

        ec_device_debug(device);

   375

        ec_device_call_isr(device); // Empfangenes "vergessen"

   376

        return -1;

   377

    }

   378

   379

    // Empfangene Daten in Kommandodatenspeicher kopieren

   380

    memcpy(frame->data, data, data_length);

   381

    data += data_length;

   382

   383

    // Working-Counter setzen

   384

    frame->working_counter = EC_READ_U16(data);

   385

   386

    return 0;

   387

}

   388

   389

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

   390

   391

/**

   392

   Sendet einen einzeln Rahmen und wartet auf dessen Empfang.

   393

   394

   Wenn der Working-Counter nicht gesetzt wurde, wird der Rahmen

   395

   nochmals gesendet.

   396

   397

   \todo Das ist noch nicht schön, da hier zwei Protokollschichten

   398

   vermischt werden.

   399

   400

   \return 0 bei Erfolg, sonst < 0

   401

*/

   402

   403

int ec_frame_send_receive(ec_frame_t *frame

   404

                          /**< Rahmen zum Senden/Empfangen */

   405

                          )

   406

{

   407

    unsigned int timeout_tries_left, response_tries_left;

   408

    unsigned int tries;

   409

   410

    tries = 0;

   411

    response_tries_left = 10;

   412

    do

   413

    {

   414

        tries++;

   415

        if (unlikely(ec_frame_send(frame) < 0)) {

   416

            EC_ERR("Frame sending failed!\n");

   417

            return -1;

   418

        }

   419

   420

        timeout_tries_left = 20;

   421

        do

   422

        {

   423

            udelay(1);

   424

            ec_device_call_isr(&frame->master->device);

   425

            timeout_tries_left--;

   426

        }

   427

        while (unlikely(!ec_device_received(&frame->master->device)

   428

                        && timeout_tries_left));

   429

   430

        if (unlikely(!timeout_tries_left)) {

   431

            EC_ERR("Frame timeout!\n");

   432

            return -1;

   433

        }

   434

   435

        if (unlikely(ec_frame_receive(frame) < 0)) {

   436

            EC_ERR("Frame receiving failed!\n");

   437

            return -1;

   438

        }

   439

   440

        response_tries_left--;

   441

    }

   442

    while (unlikely(!frame->working_counter && response_tries_left));

   443

   444

    if (unlikely(!response_tries_left)) {

   445

        EC_ERR("No response!");

   446

        return -1;

   447

    }

   448

   449

    if (tries > 1) EC_WARN("%i tries necessary...\n", tries);

   450

   451

    return 0;

   452

}

   453

   454

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

   455

   456

/* Emacs-Konfiguration

   457

;;; Local Variables: ***

   458

;;; c-basic-offset:4 ***

   459

;;; End: ***

   460

*/