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->state = ec_frame_ready; \

    23

    frame->index = 0; \

    24

    frame->working_counter = 0;

    25

    26

#define EC_FUNC_WRITE_FOOTER \

    27

    frame->data_length = length; \

    28

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

    29

    30

#define EC_FUNC_READ_FOOTER \

    31

    frame->data_length = length; \

    32

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

    33

    34

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

    35

    36

/**

    37

   Initialisiert ein EtherCAT-NPRD-Kommando.

    38

    39

   Node-adressed physical read.

    40

*/

    41

    42

void ec_frame_init_nprd(ec_frame_t *frame,

    43

                        /**< EtherCAT-Rahmen */

    44

                        ec_master_t *master,

    45

                        /**< EtherCAT-Master */

    46

                        uint16_t node_address,

    47

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

    48

                        uint16_t offset,

    49

                        /**< Physikalische Speicheradresse im Slave */

    50

                        unsigned int length

    51

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

    52

                        )

    53

{

    54

    if (unlikely(node_address == 0x0000))

    55

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

    56

    57

    EC_FUNC_HEADER;

    58

    59

    frame->type = ec_frame_type_nprd;

    60

    frame->address.physical.slave = node_address;

    61

    frame->address.physical.mem = offset;

    62

    63

    EC_FUNC_READ_FOOTER;

    64

}

    65

    66

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

    67

    68

/**

    69

   Initialisiert ein EtherCAT-NPWR-Kommando.

    70

    71

   Node-adressed physical write.

    72

*/

    73

    74

void ec_frame_init_npwr(ec_frame_t *frame,

    75

                        /**< EtherCAT-Rahmen */

    76

                        ec_master_t *master,

    77

                        /**< EtherCAT-Master */

    78

                        uint16_t node_address,

    79

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

    80

                        uint16_t offset,

    81

                        /**< Physikalische Speicheradresse im Slave */

    82

                        unsigned int length,

    83

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

    84

                        const uint8_t *data

    85

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

    86

                        )

    87

{

    88

    if (unlikely(node_address == 0x0000))

    89

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

    90

    91

    EC_FUNC_HEADER;

    92

    93

    frame->type = ec_frame_type_npwr;

    94

    frame->address.physical.slave = node_address;

    95

    frame->address.physical.mem = offset;

    96

    97

    EC_FUNC_WRITE_FOOTER;

    98

}

    99

   100

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

   101

   102

/**

   103

   Initialisiert ein EtherCAT-APRD-Kommando.

   104

   105

   Autoincrement physical read.

   106

*/

   107

   108

void ec_frame_init_aprd(ec_frame_t *frame,

   109

                        /**< EtherCAT-Rahmen */

   110

                        ec_master_t *master,

   111

                        /**< EtherCAT-Master */

   112

                        uint16_t ring_position,

   113

                        /**< Position des Slaves im Bus */

   114

                        uint16_t offset,

   115

                        /**< Physikalische Speicheradresse im Slave */

   116

                        unsigned int length

   117

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

   118

                        )

   119

{

   120

    EC_FUNC_HEADER;

   121

   122

    frame->type = ec_frame_type_aprd;

   123

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

   124

    frame->address.physical.mem = offset;

   125

   126

    EC_FUNC_READ_FOOTER;

   127

}

   128

   129

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

   130

   131

/**

   132

   Initialisiert ein EtherCAT-APWR-Kommando.

   133

   134

   Autoincrement physical write.

   135

*/

   136

   137

void ec_frame_init_apwr(ec_frame_t *frame,

   138

                        /**< EtherCAT-Rahmen */

   139

                        ec_master_t *master,

   140

                        /**< EtherCAT-Master */

   141

                        uint16_t ring_position,

   142

                        /**< Position des Slaves im Bus */

   143

                        uint16_t offset,

   144

                        /**< Physikalische Speicheradresse im Slave */

   145

                        unsigned int length,

   146

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

   147

                        const uint8_t *data

   148

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

   149

                        )

   150

{

   151

    EC_FUNC_HEADER;

   152

   153

    frame->type = ec_frame_type_apwr;

   154

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

   155

    frame->address.physical.mem = offset;

   156

   157

    EC_FUNC_WRITE_FOOTER;

   158

}

   159

   160

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

   161

   162

/**

   163

   Initialisiert ein EtherCAT-BRD-Kommando.

   164

   165

   Broadcast read.

   166

*/

   167

   168

void ec_frame_init_brd(ec_frame_t *frame,

   169

                       /**< EtherCAT-Rahmen */

   170

                       ec_master_t *master,

   171

                       /**< EtherCAT-Master */

   172

                       uint16_t offset,

   173

                       /**< Physikalische Speicheradresse im Slave */

   174

                       unsigned int length

   175

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

   176

                       )

   177

{

   178

    EC_FUNC_HEADER;

   179

   180

    frame->type = ec_frame_type_brd;

   181

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

   182

    frame->address.physical.mem = offset;

   183

   184

    EC_FUNC_READ_FOOTER;

   185

}

   186

   187

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

   188

   189

/**

   190

   Initialisiert ein EtherCAT-BWR-Kommando.

   191

   192

   Broadcast write.

   193

*/

   194

   195

void ec_frame_init_bwr(ec_frame_t *frame,

   196

                       /**< EtherCAT-Rahmen */

   197

                       ec_master_t *master,

   198

                       /**< EtherCAT-Master */

   199

                       uint16_t offset,

   200

                       /**< Physikalische Speicheradresse im Slave */

   201

                       unsigned int length,

   202

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

   203

                       const uint8_t *data

   204

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

   205

                       )

   206

{

   207

    EC_FUNC_HEADER;

   208

   209

    frame->type = ec_frame_type_bwr;

   210

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

   211

    frame->address.physical.mem = offset;

   212

   213

    EC_FUNC_WRITE_FOOTER;

   214

}

   215

   216

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

   217

   218

/**

   219

   Initialisiert ein EtherCAT-LRW-Kommando.

   220

   221

   Logical read write.

   222

*/

   223

   224

void ec_frame_init_lrw(ec_frame_t *frame,

   225

                       /**< EtherCAT-Rahmen */

   226

                       ec_master_t *master,

   227

                       /**< EtherCAT-Master */

   228

                       uint32_t offset,

   229

                       /**< Logische Startadresse */

   230

                       unsigned int length,

   231

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

   232

                       uint8_t *data

   233

                       /**< Zeiger auf die Daten */

   234

                       )

   235

{

   236

    EC_FUNC_HEADER;

   237

   238

    frame->type = ec_frame_type_lrw;

   239

    frame->address.logical = offset;

   240

   241

    EC_FUNC_WRITE_FOOTER;

   242

}

   243

   244

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

   245

   246

/**

   247

   Sendet einen einzelnen EtherCAT-Rahmen.

   248

   249

   \return 0 bei Erfolg, sonst < 0

   250

*/

   251

   252

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

   253

{

   254

    unsigned int command_size, frame_size, i;

   255

    uint8_t *data;

   256

   257

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

   258

        EC_DBG("ec_frame_send\n");

   259

    }

   260

   261

    if (unlikely(frame->state != ec_frame_ready)) {

   262

        EC_WARN("Frame not in \"ready\" state!\n");

   263

    }

   264

   265

    command_size = frame->data_length + EC_COMMAND_HEADER_SIZE

   266

        + EC_COMMAND_FOOTER_SIZE;

   267

    frame_size = command_size + EC_FRAME_HEADER_SIZE;

   268

   269

    if (unlikely(frame_size > EC_MAX_FRAME_SIZE)) {

   270

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

   271

        return -1;

   272

    }

   273

   274

    if (frame_size < EC_MIN_FRAME_SIZE) frame_size = EC_MIN_FRAME_SIZE;

   275

   276

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

   277

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

   278

    }

   279

   280

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

   281

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

   282

   283

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

   284

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

   285

    }

   286

   287

    frame->state = ec_frame_sent;

   288

   289

    // Zeiger auf Socket-Buffer holen

   290

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

   291

   292

    // EtherCAT frame header

   293

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

   294

    data += EC_FRAME_HEADER_SIZE;

   295

   296

    // EtherCAT command header

   297

    EC_WRITE_U8 (data,     frame->type);

   298

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

   299

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

   300

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

   301

    EC_WRITE_U16(data + 8, 0x0000);

   302

    data += EC_COMMAND_HEADER_SIZE;

   303

   304

    // EtherCAT command data

   305

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

   306

    data += frame->data_length;

   307

   308

    // EtherCAT command footer

   309

    EC_WRITE_U16(data, frame->working_counter);

   310

    data += EC_COMMAND_FOOTER_SIZE;

   311

   312

    // Pad with zeros

   313

    for (i = EC_FRAME_HEADER_SIZE + EC_COMMAND_HEADER_SIZE

   314

             + frame->data_length + EC_COMMAND_FOOTER_SIZE;

   315

         i < EC_MIN_FRAME_SIZE; i++)

   316

        EC_WRITE_U8(data++, 0x00);

   317

   318

    // Send frame

   319

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

   320

   321

    return 0;

   322

}

   323

   324

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

   325

   326

/**

   327

   Empfängt einen gesendeten Rahmen.

   328

   329

   \return 0 bei Erfolg, sonst < 0

   330

*/

   331

   332

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

   333

{

   334

    unsigned int received_length, frame_length, data_length;

   335

    uint8_t *data;

   336

    uint8_t command_type, command_index;

   337

    ec_device_t *device;

   338

   339

    if (unlikely(frame->state != ec_frame_sent)) {

   340

        EC_ERR("Frame was not sent!\n");

   341

        return -1;

   342

    }

   343

   344

    device = &frame->master->device;

   345

   346

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

   347

   348

    device->state = EC_DEVICE_STATE_READY;

   349

   350

    if (unlikely(received_length < EC_FRAME_HEADER_SIZE)) {

   351

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

   352

        ec_device_debug(device);

   353

        return -1;

   354

    }

   355

   356

    data = ec_device_data(device);

   357

   358

    // Länge des gesamten Frames prüfen

   359

    frame_length = EC_READ_U16(data) & 0x07FF;

   360

    data += EC_FRAME_HEADER_SIZE;

   361

   362

    if (unlikely(frame_length > received_length)) {

   363

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

   364

        ec_device_debug(device);

   365

        return -1;

   366

    }

   367

   368

    // Command header

   369

    command_type =  EC_READ_U8(data);

   370

    command_index = EC_READ_U8(data + 1);

   371

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

   372

    data += EC_COMMAND_HEADER_SIZE;

   373

   374

    if (unlikely(EC_FRAME_HEADER_SIZE + EC_COMMAND_HEADER_SIZE

   375

                 + data_length + EC_COMMAND_FOOTER_SIZE > received_length)) {

   376

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

   377

        ec_device_debug(device);

   378

        return -1;

   379

    }

   380

   381

    if (unlikely(frame->type != command_type

   382

                 || frame->index != command_index

   383

                 || frame->data_length != data_length))

   384

    {

   385

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

   386

        ec_device_debug(device);

   387

        ec_device_call_isr(device); // Empfangenes "vergessen"

   388

        return -1;

   389

    }

   390

   391

    frame->state = ec_frame_received;

   392

   393

    // Empfangene Daten in Kommandodatenspeicher kopieren

   394

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

   395

    data += data_length;

   396

   397

    // Working-Counter setzen

   398

    frame->working_counter = EC_READ_U16(data);

   399

   400

    return 0;

   401

}

   402

   403

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

   404

   405

/**

   406

   Sendet einen einzeln Rahmen und wartet auf dessen Empfang.

   407

   408

   \return 0 bei Erfolg, sonst < 0

   409

*/

   410

   411

int ec_frame_send_receive(ec_frame_t *frame

   412

                          /**< Rahmen zum Senden/Empfangen */

   413

                          )

   414

{

   415

    unsigned int tries_left;

   416

   417

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

   418

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

   419

        return -1;

   420

    }

   421

   422

    tries_left = 20;

   423

    do

   424

    {

   425

        udelay(1);

   426

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

   427

        tries_left--;

   428

    }

   429

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

   430

                    && tries_left));

   431

   432

    if (unlikely(!tries_left)) {

   433

        EC_ERR("Frame timeout!\n");

   434

        return -1;

   435

    }

   436

   437

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

   438

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

   439

        return -1;

   440

    }

   441

   442

    return 0;

   443

}

   444

   445

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

   446

   447

/* Emacs-Konfiguration

   448

;;; Local Variables: ***

   449

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

   450

;;; End: ***

   451

*/