1

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

     2

 *

     3

 *  e c _ m a s t e r . c

     4

 *

     5

 *  Methoden für einen EtherCAT-Master.

     6

 *

     7

 *  $Id$

     8

 *

     9

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

    10

    11

#include <linux/module.h>

    12

#include <linux/kernel.h>

    13

#include <linux/string.h>

    14

#include <linux/slab.h>

    15

#include <linux/delay.h>

    16

    17

#include "ec_globals.h"

    18

#include "ec_master.h"

    19

    20

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

    21

    22

/**

    23

   Konstruktor des EtherCAT-Masters.

    24

    25

   @param master Zeiger auf den zu initialisierenden EtherCAT-Master

    26

*/

    27

    28

void EtherCAT_master_init(EtherCAT_master_t *master)

    29

{

    30

  master->dev = NULL;

    31

  master->command_index = 0x00;

    32

  master->tx_data_length = 0;

    33

  master->rx_data_length = 0;

    34

  master->domain_count = 0;

    35

  master->debug_level = 0;

    36

  master->tx_time = 0;

    37

  master->rx_time = 0;

    38

  master->rx_tries = 0;

    39

}

    40

    41

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

    42

    43

/**

    44

   Destruktor eines EtherCAT-Masters.

    45

    46

   Entfernt alle Kommandos aus der Liste, löscht den Zeiger

    47

   auf das Slave-Array und gibt die Prozessdaten frei.

    48

    49

   @param master Zeiger auf den zu löschenden Master

    50

*/

    51

    52

void EtherCAT_master_clear(EtherCAT_master_t *master)

    53

{

    54

  unsigned int i;

    55

    56

  // Remove domains

    57

  for (i = 0; i < master->domain_count; i++) {

    58

    EtherCAT_domain_clear(master->domains + i);

    59

  }

    60

    61

  master->domain_count = 0;

    62

}

    63

    64

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

    65

    66

/**

    67

   Öffnet ein EtherCAT-Geraet für den Master.

    68

    69

   Registriert das Geraet beim Master, der es daraufhin oeffnet.

    70

    71

   @param master Der EtherCAT-Master

    72

   @param device Das EtherCAT-Geraet

    73

   @return 0, wenn alles o.k.,

    74

   < 0, wenn bereits ein Geraet registriert

    75

   oder das Geraet nicht geoeffnet werden konnte.

    76

*/

    77

    78

int EtherCAT_master_open(EtherCAT_master_t *master,

    79

                         EtherCAT_device_t *device)

    80

{

    81

  if (!master || !device) {

    82

    printk(KERN_ERR "EtherCAT: Illegal parameters for master_open()!\n");

    83

    return -1;

    84

  }

    85

    86

  if (master->dev) {

    87

    printk(KERN_ERR "EtherCAT: Master already has a device.\n");

    88

    return -1;

    89

  }

    90

    91

  if (EtherCAT_device_open(device) < 0) {

    92

    printk(KERN_ERR "EtherCAT: Could not open device %X!\n",

    93

           (unsigned int) master->dev);

    94

    return -1;

    95

  }

    96

    97

  master->dev = device;

    98

    99

  return 0;

   100

}

   101

   102

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

   103

   104

/**

   105

   Schliesst das EtherCAT-Geraet, auf dem der Master arbeitet.

   106

   107

   @param master Der EtherCAT-Master

   108

   @param device Das EtherCAT-Geraet

   109

*/

   110

   111

void EtherCAT_master_close(EtherCAT_master_t *master,

   112

                           EtherCAT_device_t *device)

   113

{

   114

  if (master->dev != device) {

   115

    printk(KERN_WARNING "EtherCAT: Warning -"

   116

           " Trying to close an unknown device!\n");

   117

    return;

   118

  }

   119

   120

  if (EtherCAT_device_close(master->dev) < 0) {

   121

    printk(KERN_WARNING "EtherCAT: Warning -"

   122

           " Could not close device!\n");

   123

  }

   124

   125

  master->dev = NULL;

   126

}

   127

   128

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

   129

   130

/**

   131

   Sendet ein einzelnes Kommando in einem Frame und

   132

   wartet auf dessen Empfang.

   133

   134

   @param master EtherCAT-Master

   135

   @param cmd    Kommando zum Senden/Empfangen

   136

   137

   @return 0 bei Erfolg, sonst < 0

   138

*/

   139

   140

int EtherCAT_simple_send_receive(EtherCAT_master_t *master,

   141

                                 EtherCAT_command_t *cmd)

   142

{

   143

  unsigned int tries_left;

   144

   145

  if (unlikely(EtherCAT_simple_send(master, cmd) < 0))

   146

    return -1;

   147

   148

  udelay(3);

   149

   150

  EtherCAT_device_call_isr(master->dev);

   151

   152

  tries_left = 20;

   153

  while (unlikely(master->dev->state == ECAT_DS_SENT

   154

                  && tries_left)) {

   155

    udelay(1);

   156

    EtherCAT_device_call_isr(master->dev);

   157

    tries_left--;

   158

  }

   159

   160

  if (unlikely(EtherCAT_simple_receive(master, cmd) < 0))

   161

    return -1;

   162

   163

  return 0;

   164

}

   165

   166

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

   167

   168

/**

   169

   Sendet ein einzelnes Kommando in einem Frame.

   170

   171

   @param master EtherCAT-Master

   172

   @param cmd    Kommando zum Senden

   173

   174

   @return 0 bei Erfolg, sonst < 0

   175

*/

   176

   177

int EtherCAT_simple_send(EtherCAT_master_t *master,

   178

                         EtherCAT_command_t *cmd)

   179

{

   180

  unsigned int length, framelength, i;

   181

   182

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

   183

    printk(KERN_DEBUG "EtherCAT_send_receive_command\n");

   184

  }

   185

   186

  if (unlikely(cmd->state != ECAT_CS_READY)) {

   187

    printk(KERN_WARNING "EtherCAT_send_receive_command:"

   188

           "Command not in ready state!\n");

   189

  }

   190

   191

  length = cmd->data_length + 12;

   192

  framelength = length + 2;

   193

   194

  if (unlikely(framelength > ECAT_FRAME_BUFFER_SIZE)) {

   195

    printk(KERN_ERR "EtherCAT: Frame too long (%i)!\n", framelength);

   196

    return -1;

   197

  }

   198

   199

  if (framelength < 46) framelength = 46;

   200

   201

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

   202

    printk(KERN_DEBUG "Frame length: %i\n", framelength);

   203

  }

   204

   205

  master->tx_data[0] = length & 0xFF;

   206

  master->tx_data[1] = ((length & 0x700) >> 8) | 0x10;

   207

   208

  cmd->index = master->command_index;

   209

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

   210

   211

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

   212

    printk(KERN_DEBUG "Sending command index %i\n", cmd->index);

   213

  }

   214

   215

  cmd->state = ECAT_CS_SENT;

   216

   217

  master->tx_data[2 + 0] = cmd->type;

   218

  master->tx_data[2 + 1] = cmd->index;

   219

  master->tx_data[2 + 2] = cmd->address.raw[0];

   220

  master->tx_data[2 + 3] = cmd->address.raw[1];

   221

  master->tx_data[2 + 4] = cmd->address.raw[2];

   222

  master->tx_data[2 + 5] = cmd->address.raw[3];

   223

  master->tx_data[2 + 6] = cmd->data_length & 0xFF;

   224

  master->tx_data[2 + 7] = (cmd->data_length & 0x700) >> 8;

   225

  master->tx_data[2 + 8] = 0x00;

   226

  master->tx_data[2 + 9] = 0x00;

   227

   228

  if (likely(cmd->type == ECAT_CMD_APWR

   229

             || cmd->type == ECAT_CMD_NPWR

   230

             || cmd->type == ECAT_CMD_BWR

   231

             || cmd->type == ECAT_CMD_LRW)) // Write commands

   232

  {

   233

    for (i = 0; i < cmd->data_length; i++)

   234

      master->tx_data[2 + 10 + i] = cmd->data[i];

   235

  }

   236

  else // Read commands

   237

  {

   238

    for (i = 0; i < cmd->data_length; i++) master->tx_data[2 + 10 + i] = 0x00;

   239

  }

   240

   241

  master->tx_data[2 + 10 + cmd->data_length] = 0x00;

   242

  master->tx_data[2 + 11 + cmd->data_length] = 0x00;

   243

   244

  // Pad with zeros

   245

  for (i = cmd->data_length + 12 + 2; i < 46; i++) master->tx_data[i] = 0x00;

   246

   247

  master->tx_data_length = framelength;

   248

   249

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

   250

    printk(KERN_DEBUG "device send...\n");

   251

  }

   252

   253

  // Zeit nehmen

   254

  rdtscl(master->tx_time);

   255

   256

  // Send frame

   257

  if (unlikely(EtherCAT_device_send(master->dev,

   258

                                    master->tx_data,

   259

                                    framelength) != 0)) {

   260

    printk(KERN_ERR "EtherCAT: Could not send!\n");

   261

    return -1;

   262

  }

   263

   264

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

   265

    printk(KERN_DEBUG "EtherCAT_send done.\n");

   266

  }

   267

   268

  return 0;

   269

}

   270

   271

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

   272

   273

/**

   274

   Wartet auf den Empfang eines einzeln gesendeten

   275

   Kommandos.

   276

   277

   @param master EtherCAT-Master

   278

   @param cmd    Gesendetes Kommando

   279

   280

   @return 0 bei Erfolg, sonst < 0

   281

*/

   282

   283

int EtherCAT_simple_receive(EtherCAT_master_t *master,

   284

                            EtherCAT_command_t *cmd)

   285

{

   286

  unsigned int length;

   287

  int ret;

   288

  unsigned char command_type, command_index;

   289

   290

  if (unlikely((ret = EtherCAT_device_receive(master->dev,

   291

                                              master->rx_data)) < 0))

   292

    return -1;

   293

   294

  master->rx_data_length = (unsigned int) ret;

   295

   296

  if (unlikely(master->rx_data_length < 2)) {

   297

    printk(KERN_ERR "EtherCAT: Received frame with incomplete"

   298

           " EtherCAT header!\n");

   299

    output_debug_data(master);

   300

    return -1;

   301

  }

   302

   303

  // Länge des gesamten Frames prüfen

   304

  length = ((master->rx_data[1] & 0x07) << 8)

   305

    | (master->rx_data[0] & 0xFF);

   306

   307

  if (unlikely(length > master->rx_data_length)) {

   308

    printk(KERN_ERR "EtherCAT: Received corrupted frame (length does"

   309

           " not match)!\n");

   310

    output_debug_data(master);

   311

    return -1;

   312

  }

   313

   314

  command_type = master->rx_data[2];

   315

  command_index = master->rx_data[2 + 1];

   316

  length = (master->rx_data[2 + 6] & 0xFF)

   317

    | ((master->rx_data[2 + 7] & 0x07) << 8);

   318

   319

  if (unlikely(master->rx_data_length - 2 < length + 12)) {

   320

    printk(KERN_ERR "EtherCAT: Received frame with"

   321

           " incomplete command data!\n");

   322

    output_debug_data(master);

   323

    return -1;

   324

  }

   325

   326

  if (likely(cmd->state == ECAT_CS_SENT

   327

             && cmd->type == command_type

   328

             && cmd->index == command_index

   329

             && cmd->data_length == length))

   330

  {

   331

    cmd->state = ECAT_CS_RECEIVED;

   332

   333

    // Empfangene Daten in Kommandodatenspeicher kopieren

   334

    memcpy(cmd->data, master->rx_data + 2 + 10, length);

   335

   336

    // Working-Counter setzen

   337

    cmd->working_counter

   338

      = ((master->rx_data[length + 2 + 10] & 0xFF)

   339

         | ((master->rx_data[length + 2 + 11] & 0xFF) << 8));

   340

  }

   341

  else

   342

  {

   343

    printk(KERN_WARNING "EtherCAT: WARNING - Send/Receive anomaly!\n");

   344

    output_debug_data(master);

   345

  }

   346

   347

  master->dev->state = ECAT_DS_READY;

   348

   349

  return 0;

   350

}

   351

   352

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

   353

   354

/**

   355

   Überprüft die angeschlossenen Slaves.

   356

   357

   Vergleicht die an den Bus angeschlossenen Slaves mit

   358

   den im statischen-Slave-Array vorgegebenen Konfigurationen.

   359

   Stimmen Anzahl oder Typen nicht überein, gibt diese

   360

   Methode einen Fehler aus.

   361

   362

   @param master Der EtherCAT-Master

   363

   @param slaves Zeiger auf ein statisches Slave-Array

   364

   @param slave_count Anzahl der Slaves im Array

   365

   366

   @return 0 bei Erfolg, sonst < 0

   367

*/

   368

   369

int EtherCAT_check_slaves(EtherCAT_master_t *master,

   370

                          EtherCAT_slave_t *slaves,

   371

                          unsigned int slave_count)

   372

{

   373

  EtherCAT_command_t cmd;

   374

  EtherCAT_slave_t *cur;

   375

  unsigned int i, j, found, size, offset;

   376

  unsigned char data[2];

   377

  EtherCAT_domain_t *dom;

   378

   379

  // Clear domains

   380

  for (i = 0; i < master->domain_count; i++) {

   381

    printk(KERN_DEBUG "EtherCAT: Clearing domain %i!\n",

   382

           master->domains[i].number);

   383

    EtherCAT_domain_clear(master->domains + i);

   384

  }

   385

  master->domain_count = 0;

   386

   387

  if (unlikely(!slave_count)) {

   388

    printk(KERN_ERR "EtherCAT: No slaves in list!\n");

   389

    return -1;

   390

  }

   391

   392

  // Determine number of slaves on bus

   393

   394

  EtherCAT_command_broadcast_read(&cmd, 0x0000, 4);

   395

   396

  if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))

   397

    return -1;

   398

   399

  if (unlikely(cmd.working_counter != slave_count)) {

   400

    printk(KERN_ERR "EtherCAT: Wrong number of slaves on bus: %i / %i\n",

   401

           cmd.working_counter, slave_count);

   402

    return -1;

   403

  }

   404

   405

  printk("EtherCAT: Found all %i slaves.\n", slave_count);

   406

   407

  // For every slave in the list

   408

  for (i = 0; i < slave_count; i++)

   409

  {

   410

    cur = &slaves[i];

   411

   412

    if (unlikely(!cur->desc)) {

   413

      printk(KERN_ERR "EtherCAT: Slave %i has no description.\n", i);

   414

      return -1;

   415

    }

   416

   417

    // Set ring position

   418

    cur->ring_position = -i;

   419

    cur->station_address = i + 1;

   420

   421

    // Write station address

   422

   423

    data[0] = cur->station_address & 0x00FF;

   424

    data[1] = (cur->station_address & 0xFF00) >> 8;

   425

   426

    EtherCAT_command_position_write(&cmd, cur->ring_position,

   427

                                    0x0010, 2, data);

   428

   429

    if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))

   430

      return -1;

   431

   432

    if (unlikely(cmd.working_counter != 1)) {

   433

      printk(KERN_ERR "EtherCAT: Slave %i did not repond"

   434

             " while writing station address!\n", i);

   435

      return -1;

   436

    }

   437

   438

    // Read base data

   439

   440

    EtherCAT_command_read(&cmd, cur->station_address, 0x0000, 4);

   441

   442

    if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))

   443

      return -1;

   444

   445

    if (unlikely(cmd.working_counter != 1)) {

   446

      printk(KERN_ERR "EtherCAT: Slave %i did not respond"

   447

             " while reading base data!\n", i);

   448

      return -1;

   449

    }

   450

   451

    // Get base data

   452

   453

    cur->type = cmd.data[0];

   454

    cur->revision = cmd.data[1];

   455

    cur->build = cmd.data[2] | (cmd.data[3] << 8);

   456

   457

    // Read identification from "Slave Information Interface" (SII)

   458

   459

    if (unlikely(EtherCAT_read_slave_information(master,

   460

                                                 cur->station_address,

   461

                                                 0x0008,

   462

                                                 &cur->vendor_id) != 0))

   463

    {

   464

      printk(KERN_ERR "EtherCAT: Could not read SII vendor id!\n");

   465

      return -1;

   466

    }

   467

   468

    if (unlikely(EtherCAT_read_slave_information(master,

   469

                                                 cur->station_address,

   470

                                                 0x000A,

   471

                                                 &cur->product_code) != 0))

   472

    {

   473

      printk(KERN_ERR "EtherCAT: Could not read SII product code!\n");

   474

      return -1;

   475

    }

   476

   477

    if (unlikely(EtherCAT_read_slave_information(master,

   478

                                                 cur->station_address,

   479

                                                 0x000C,

   480

                                                 &cur->revision_number) != 0))

   481

    {

   482

      printk(KERN_ERR "EtherCAT: Could not read SII revision number!\n");

   483

      return -1;

   484

    }

   485

   486

    if (unlikely(EtherCAT_read_slave_information(master,

   487

                                                 cur->station_address,

   488

                                                 0x000E,

   489

                                                 &cur->serial_number) != 0))

   490

    {