1

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

     2

 *

     3

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

     4

 *

     5

 *  Methoden für einen EtherCAT-Master.

     6

 *

     7

 *  $Date$

     8

 *  $Author$

     9

 *

    10

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

    11

    13

#include <linux/kernel.h>

    14

#include <linux/string.h>

    15

#include <linux/slab.h>

    16

#include <linux/delay.h>

    17

    18

#include "ec_globals.h"

    19

#include "ec_master.h"

    20

#include "ec_dbg.h"

    21

    22

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

    23

    24

/**

    25

   Konstruktor des EtherCAT-Masters.

    26

    27

   @param master Zeiger auf den zu initialisierenden

    28

   EtherCAT-Master

    29

   @param dev Zeiger auf das EtherCAT-Gerät, mit dem der

    30

   Master arbeiten soll

    31

    32

   @return 0 bei Erfolg, sonst < 0 (= dev ist NULL)

    33

*/

    34

    35

int EtherCAT_master_init(EtherCAT_master_t *master,

    36

                         EtherCAT_device_t *dev)

    37

{

    38

  if (!dev)

    39

  {

    40

    EC_DBG(KERN_ERR "EtherCAT: Master init without device!\n");

    41

    return -1;

    42

  }

    43

    44

  master->slaves = NULL;

    45

  master->slave_count = 0;

    46

  master->dev = dev;

    47

  master->command_index = 0x00;

    48

  master->tx_data_length = 0;

    49

  master->process_data = NULL;

    50

  master->process_data_length = 0;

    51

  master->debug_level = 0;

    52

    53

  return 0;

    54

}

    55

    56

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

    57

    58

/**

    59

   Destruktor eines EtherCAT-Masters.

    60

    61

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

    62

   auf das Slave-Array und gibt die Prozessdaten frei.

    63

    64

   @param master Zeiger auf den zu löschenden Master

    65

*/

    66

    67

void EtherCAT_master_clear(EtherCAT_master_t *master)

    68

{

    69

  // Remove all slaves

    70

  EtherCAT_clear_slaves(master);

    71

    72

  if (master->process_data)

    73

  {

    74

    kfree(master->process_data);

    75

    master->process_data = NULL;

    76

  }

    77

    78

  master->process_data_length = 0;

    79

}

    80

    81

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

    82

    83

/**

    84

   Sendet ein einzelnes Kommando in einem Frame und

    85

   wartet auf dessen Empfang.

    86

    87

   @param master EtherCAT-Master

    88

   @param cmd    Kommando zum Senden/Empfangen

    89

    90

   @return 0 bei Erfolg, sonst < 0

    91

*/

    92

    93

int EtherCAT_simple_send_receive(EtherCAT_master_t *master,

    94

                                 EtherCAT_command_t *cmd)

    95

{

    96

  unsigned int tries_left;

    97

   100

  if (EtherCAT_simple_send(master, cmd) < 0) return -1;

   101

   105

  EtherCAT_device_call_isr(master->dev);

   106

   108

  while (master->dev->state == ECAT_DS_SENT && tries_left)

   109

  {

   110

    udelay(1);

   112

    EtherCAT_device_call_isr(master->dev);

   113

    tries_left--;

   114

  }

   115

   118

  if (EtherCAT_simple_receive(master, cmd) < 0) return -1;

   119

   122

  return 0;

   123

}

   124

   125

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

   126

   127

/**

   128

   Sendet ein einzelnes Kommando in einem Frame.

   129

   130

   @param master EtherCAT-Master

   131

   @param cmd    Kommando zum Senden

   132

   133

   @return 0 bei Erfolg, sonst < 0

   134

*/

   135

   136

int EtherCAT_simple_send(EtherCAT_master_t *master,

   137

                         EtherCAT_command_t *cmd)

   138

{

   139

  unsigned int length, framelength, i;

   140

   141

  if (master->debug_level > 0)

   142

  {

   143

    EC_DBG(KERN_DEBUG "EtherCAT_send_receive_command\n");

   144

  }

   145

   146

  if (cmd->state != ECAT_CS_READY)

   147

  {

   148

    EC_DBG(KERN_WARNING "EtherCAT_send_receive_command: Command not in ready state!\n");

   149

  }

   150

   151

  length = cmd->data_length + 12;

   152

  framelength = length + 2;

   153

   154

  if (framelength > ECAT_FRAME_BUFFER_SIZE)

   155

  {

   156

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

   157

    return -1;

   158

  }

   159

   160

  if (framelength < 46) framelength = 46;

   161

   162

  if (master->debug_level > 0)

   163

  {

   164

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

   165

  }

   166

   167

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

   168

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

   169

   170

  cmd->index = master->command_index;

   171

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

   172

   173

  if (master->debug_level > 0)

   174

  {

   175

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

   176

  }

   177

   178

  cmd->state = ECAT_CS_SENT;

   179

   180

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

   181

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

   182

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

   183

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

   184

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

   185

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

   186

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

   187

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

   188

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

   189

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

   190

   191

  if (cmd->type == ECAT_CMD_APWR

   192

      || cmd->type == ECAT_CMD_NPWR

   193

      || cmd->type == ECAT_CMD_BWR

   194

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

   195

  {

   196

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

   197

  }

   198

  else // Read

   199

  {

   200

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

   201

  }

   202

   203

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

   204

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

   205

   206

  // Pad with zeros

   207

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

   208

   209

  master->tx_data_length = framelength;

   210

   211

  if (master->debug_level > 0)

   212

  {

   213

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

   214

  }

   215

   216

  // Send frame

   217

  if (EtherCAT_device_send(master->dev, master->tx_data, framelength) != 0)

   218

  {

   219

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

   220

    return -1;

   221

  }

   222

   223

  if (master->debug_level > 0)

   224

  {

   225

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

   226

  }

   227

   228

  return 0;

   229

}

   230

   231

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

   232

   233

/**

   234

   Wartet auf den Empfang eines einzeln gesendeten

   235

   Kommandos.

   236

   237

   @param master EtherCAT-Master

   238

   @param cmd    Gesendetes Kommando

   239

   240

   @return 0 bei Erfolg, sonst < 0

   241

*/

   242

   243

int EtherCAT_simple_receive(EtherCAT_master_t *master,

   244

                            EtherCAT_command_t *cmd)

   245

{

   246

  unsigned int length;

   247

  int receive_ret;

   248

  unsigned char command_type, command_index;

   249

   250

  if ((receive_ret = EtherCAT_device_receive(master->dev,

   251

                                             master->rx_data)) < 0)

   252

  {

   253

    return -1;

   254

  }

   255

   256

  master->rx_data_length = (unsigned int) receive_ret;

   257

   258

  if (master->rx_data_length < 2)

   259

  {

   260

    EC_DBG(KERN_ERR "EtherCAT: Received frame with incomplete EtherCAT header!\n");

   261

    output_debug_data(master);

   262

    return -1;

   263

  }

   264

   265

  // Länge des gesamten Frames prüfen

   266

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

   267

   268

  if (length > master->rx_data_length)

   269

  {

   270

    EC_DBG(KERN_ERR "EtherCAT: Received corrupted frame (length does not match)!\n");

   271

    output_debug_data(master);

   272

    return -1;

   273

  }

   274

   275

  command_type = master->rx_data[2];

   276

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

   277

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

   278

   279

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

   280

  {

   281

    EC_DBG(KERN_ERR "EtherCAT: Received frame with incomplete command data!\n");

   282

    output_debug_data(master);

   283

    return -1;

   284

  }

   285

   286

  if (cmd->state == ECAT_CS_SENT

   287

      && cmd->type == command_type

   288

      && cmd->index == command_index

   289

      && cmd->data_length == length)

   290

  {

   291

    cmd->state = ECAT_CS_RECEIVED;

   292

   293

    // Empfangene Daten in Kommandodatenspeicher kopieren

   294

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

   295

   296

    // Working-Counter setzen

   297

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

   298

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

   299

  }

   300

  else

   301

  {

   302

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

   303

    output_debug_data(master);

   304

  }

   305

   306

  master->dev->state = ECAT_DS_READY;

   307

   308

  return 0;

   309

}

   310

   311

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

   312

   313

/**

   314

   Überprüft die angeschlossenen Slaves.

   315

   316

   Vergleicht die an den Bus angeschlossenen Slaves mit

   317

   den im statischen-Slave-Array vorgegebenen Konfigurationen.

   318

   Stimmen Anzahl oder Typen nicht überein, gibt diese

   319

   Methode einen Fehler aus.

   320

   321

   @param master Der EtherCAT-Master

   322

   @param slaves Zeiger auf ein statisches Slave-Array

   323

   @param slave_count Anzahl der Slaves im Array

   324

   325

   @return 0 bei Erfolg, sonst < 0

   326

*/

   327

   328

int EtherCAT_check_slaves(EtherCAT_master_t *master,

   329

                          EtherCAT_slave_t *slaves,

   330

                          unsigned int slave_count)

   331

{

   332

  EtherCAT_command_t cmd;

   333

  EtherCAT_slave_t *cur;

   334

  unsigned int i, j, found, length, pos;

   335

  unsigned char data[2];

   336

   337

  // EtherCAT_clear_slaves() must be called before!

   338

  if (master->slaves || master->slave_count)

   339

  {

   340

    EC_DBG(KERN_ERR "EtherCAT duplicate slave check!");

   341

    return -1;

   342

  }

   343

   344

  // No slaves.

   345

  if (slave_count == 0)

   346

  {

   347

    EC_DBG(KERN_ERR "EtherCAT: No slaves in list!");

   348

    return -1;

   349

  }

   350

   351

  // Determine number of slaves on bus

   352

   353

  EtherCAT_command_broadcast_read(&cmd, 0x0000, 4);

   354

   355

  if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1;

   356

   357

  if (cmd.working_counter != slave_count)

   358

  {

   359

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

   360

           cmd.working_counter, slave_count);

   361

    return -1;

   362

  }

   363

  else

   364

  {

   365

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

   366

  }

   367

   368

  // For every slave in the list

   369

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

   370

  {

   371

    cur = &slaves[i];

   372

   373

    if (!cur->desc)

   374

    {

   375

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

   376

      return -1;

   377

    }

   378

   379

    // Set ring position

   380

    cur->ring_position = -i;

   381

    cur->station_address = i + 1;

   382

   383

    // Write station address

   384

   385

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

   386

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

   387

   388

    EtherCAT_command_position_write(&cmd, cur->ring_position, 0x0010, 2, data);

   389

    if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1;

   390

   391

    if (cmd.working_counter != 1)

   392

    {

   393

      EC_DBG(KERN_ERR "EtherCAT: Slave %i did not repond while writing station address!\n", i);

   394

      return -1;

   395

    }

   396

   397

    // Read base data

   398

   399

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

   400

    if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1;

   401

   402

    if (cmd.working_counter != 1)

   403

    {

   404

      EC_DBG(KERN_ERR "EtherCAT: Slave %i did not respond while reading base data!\n", i);

   405

      return -1;

   406

    }

   407

   408

    // Get base data

   409

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

   410

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

   411

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

   412

   413

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

   414

   415

    if (EtherCAT_read_slave_information(master, cur->station_address,

   416

                                        0x0008, &cur->vendor_id) != 0)

   417

    {

   418

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

   419

      return -1;

   420

    }

   421

   422

    if (EtherCAT_read_slave_information(master, cur->station_address,

   423

                                        0x000A, &cur->product_code) != 0)

   424

    {

   425

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

   426

      return -1;

   427

    }

   428

   429

    if (EtherCAT_read_slave_information(master, cur->station_address,

   430

                                        0x000C, &cur->revision_number) != 0)

   431

    {

   432

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

   433

      return -1;

   434

    }

   435

   436

    if (EtherCAT_read_slave_information(master, cur->station_address,

   437

                                        0x000E, &cur->serial_number) != 0)

   438

    {

   439

      EC_DBG(KERN_ERR "EtherCAT: Could not read SII serial number!\n");

   440

      return -1;

   441

    }

   442

   443

    // Search for identification in "database"

   444

   445

    found = 0;

   446

   447

    for (j = 0; j < slave_idents_count; j++)

   448

    {

   449

      if (slave_idents[j].vendor_id == cur->vendor_id

   450

          && slave_idents[j].product_code == cur->product_code)

   451

      {

   452

        found = 1;

   453

   454

        if (cur->desc != slave_idents[j].desc)

   455

        {

   456

          EC_DBG(KERN_ERR "EtherCAT: Unexpected slave device \"%s %s\""

   457

                 " at position %i. Expected: \"%s %s\"\n",

   458

                 slave_idents[j].desc->vendor_name,

   459

                 slave_idents[j].desc->product_name, i,

   460

                 cur->desc->vendor_name, cur->desc->product_name);

   461

          return -1;

   462

        }

   463

   464

        break;

   465

      }

   466

    }

   467

   468

    if (!found)

   469

    {

   470

      EC_DBG(KERN_ERR "EtherCAT: Unknown slave device"

   471

             " (vendor %X, code %X) at position %i.\n",

   472

             i, cur->vendor_id, cur->product_code);

   473

      return -1;

   474

    }

   475

  }

   476

   477

  length = 0;

   478

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

   479

  {

   480

    length += slaves[i].desc->data_length;

   481

  }

   482

   483

  if ((master->process_data = (unsigned char *)

   484

       kmalloc(sizeof(unsigned char) * length, GFP_KERNEL)) == NULL)

   485

  {

   486

    EC_DBG(KERN_ERR "EtherCAT: Could not allocate %i bytes for process data.\n", length);

   487

    return -1;

   488

  }

   489

   490

  master->process_data_length = length;

   491

  memset(master->process_data, 0x00, length);

   492

   493

  pos = 0;

   494

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

   495

  {

   496

    slaves[i].process_data = master->process_data + pos;

   497

    slaves[i].logical_address0 = pos;

   498

   499

    EC_DBG(KERN_DEBUG "EtherCAT: Slave %i - Address 0x%X, \"%s %s\", s/n %u\n",