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

    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

#include "ec_dbg.h"

    20

    21

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

    22

    23

/**

    24

   Konstruktor des EtherCAT-Masters.

    25

    26

   @param master Zeiger auf den zu initialisierenden

    27

   EtherCAT-Master

    28

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

    29

   Master arbeiten soll

    30

    31

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

    32

*/

    33

    34

int EtherCAT_master_init(EtherCAT_master_t *master,

    35

                         EtherCAT_device_t *dev)

    36

{

    37

  if (!dev)

    38

  {

    39

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

    40

    return -1;

    41

  }

    42

    43

  master->slaves = NULL;

    44

  master->slave_count = 0;

    45

  master->dev = dev;

    46

  master->command_index = 0x00;

    47

  master->tx_data_length = 0;

    48

  master->process_data = NULL;

    49

  master->process_data_length = 0;

    50

  master->debug_level = 0;

    51

    52

  return 0;

    53

}

    54

    57

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

    58

    59

/**

    60

   Destruktor eines EtherCAT-Masters.

    61

    62

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

    63

   auf das Slave-Array und gibt die Prozessdaten frei.

    64

    65

   @param master Zeiger auf den zu löschenden Master

    66

*/

    67

    68

void EtherCAT_master_clear(EtherCAT_master_t *master)

    69

{

    70

  // Remove all slaves

    71

  EtherCAT_clear_slaves(master);

    72

    73

  if (master->process_data)

    74

  {

    75

    kfree(master->process_data);

    76

    master->process_data = NULL;

    77

  }

    78

    79

  master->process_data_length = 0;

    80

}

    81

    84

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

    85

    86

/**

    87

   Sendet ein einzelnes Kommando in einem Frame und

    88

   wartet auf dessen Empfang.

    89

    90

   @param master EtherCAT-Master

    91

   @param cmd    Kommando zum Senden/Empfangen

    92

    93

   @return 0 bei Erfolg, sonst < 0

    94

*/

    95

    96

int EtherCAT_simple_send_receive(EtherCAT_master_t *master,

    97

                                 EtherCAT_command_t *cmd)

    98

{

    99

  unsigned int tries_left;

   100

   101

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

   102

   103

  EtherCAT_device_call_isr(master->dev);

   104

   105

  tries_left = 1000;

   106

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

   107

  {

   108

    udelay(1);

   109

    EtherCAT_device_call_isr(master->dev);

   110

    tries_left--;

   111

  }

   112

   113

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

   114

   115

  return 0;

   116

}

   117

   118

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

   119

   120

/**

   121

   Sendet ein einzelnes Kommando in einem Frame.

   122

   123

   @param master EtherCAT-Master

   124

   @param cmd    Kommando zum Senden

   125

   126

   @return 0 bei Erfolg, sonst < 0

   127

*/

   128

   129

int EtherCAT_simple_send(EtherCAT_master_t *master,

   130

                         EtherCAT_command_t *cmd)

   131

{

   132

  unsigned int length, framelength, i;

   133

   134

  if (master->debug_level > 0)

   135

  {

   136

    EC_DBG(KERN_DEBUG "EtherCAT_send_receive_command\n");

   137

  }

   138

   139

  if (cmd->state != ECAT_CS_READY)

   140

  {

   141

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

   142

  }

   143

   144

  length = cmd->data_length + 12;

   145

  framelength = length + 2;

   146

   147

  if (framelength > ECAT_FRAME_BUFFER_SIZE)

   148

  {

   149

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

   150

    return -1;

   151

  }

   152

   153

  if (framelength < 46) framelength = 46;

   154

   155

  if (master->debug_level > 0)

   156

  {

   157

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

   158

  }

   159

   160

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

   161

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

   162

   163

  cmd->index = master->command_index;

   164

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

   165

   166

  if (master->debug_level > 0)

   167

  {

   168

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

   169

  }

   170

   171

  cmd->state = ECAT_CS_SENT;

   172

   173

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

   174

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

   175

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

   176

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

   177

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

   178

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

   179

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

   180

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

   181

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

   182

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

   183

   184

  if (cmd->type == ECAT_CMD_APWR

   185

      || cmd->type == ECAT_CMD_NPWR

   186

      || cmd->type == ECAT_CMD_BWR

   187

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

   188

  {

   189

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

   190

  }

   191

  else // Read

   192

  {

   193

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

   194

  }

   195

   196

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

   197

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

   198

   199

  // Pad with zeros

   200

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

   201

   202

  master->tx_data_length = framelength;

   203

   204

  if (master->debug_level > 0)

   205

  {

   206

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

   207

  }

   208

   209

  // Send frame

   210

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

   211

  {

   212

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

   213

    return -1;

   214

  }

   215

   216

  if (master->debug_level > 0)

   217

  {

   218

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

   219

  }

   220

   221

  return 0;

   222

}

   223

   224

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

   225

   226

/**

   227

   Wartet auf den Empfang eines einzeln gesendeten

   228

   Kommandos.

   229

   230

   @param master EtherCAT-Master

   231

   @param cmd    Gesendetes Kommando

   232

   233

   @return 0 bei Erfolg, sonst < 0

   234

*/

   235

   236

int EtherCAT_simple_receive(EtherCAT_master_t *master,

   237

                            EtherCAT_command_t *cmd)

   238

{

   239

  unsigned int length;

   240

  int receive_ret;

   241

  unsigned char command_type, command_index;

   242

   243

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

   244

                                             master->rx_data)) < 0)

   245

  {

   246

    return -1;

   247

  }

   248

   249

  master->rx_data_length = (unsigned int) receive_ret;

   250

   251

  if (master->rx_data_length < 2)

   252

  {

   253

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

   254

    output_debug_data(master);

   255

    return -1;

   256

  }

   257

   258

  // Länge des gesamten Frames prüfen

   259

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

   260

   261

  if (length > master->rx_data_length)

   262

  {

   263

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

   264

    output_debug_data(master);

   265

    return -1;

   266

  }

   267

   268

  command_type = master->rx_data[2];

   269

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

   270

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

   271

   272

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

   273

  {

   274

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

   275

    output_debug_data(master);

   276

    return -1;

   277

  }

   278

   279

  if (cmd->state == ECAT_CS_SENT

   280

      && cmd->type == command_type

   281

      && cmd->index == command_index

   282

      && cmd->data_length == length)

   283

  {

   284

    cmd->state = ECAT_CS_RECEIVED;

   285

   286

    // Empfangene Daten in Kommandodatenspeicher kopieren

   287

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

   288

   289

    // Working-Counter setzen

   290

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

   291

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

   292

  }

   293

  else

   294

  {

   295

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

   296

    output_debug_data(master);

   297

  }

   298

   299

  master->dev->state = ECAT_DS_READY;

   300

   301

  return 0;

   302

}

   303

   304

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

   305

   306

/**

   307

   Überprüft die angeschlossenen Slaves.

   308

   309

   Vergleicht die an den Bus angeschlossenen Slaves mit

   310

   den im statischen-Slave-Array vorgegebenen Konfigurationen.

   311

   Stimmen Anzahl oder Typen nicht überein, gibt diese

   312

   Methode einen Fehler aus.

   313

   314

   @param master Der EtherCAT-Master

   315

   @param slaves Zeiger auf ein statisches Slave-Array

   316

   @param slave_count Anzahl der Slaves im Array

   317

   318

   @return 0 bei Erfolg, sonst < 0

   319

*/

   320

   321

int EtherCAT_check_slaves(EtherCAT_master_t *master,

   322

                          EtherCAT_slave_t *slaves,

   323

                          unsigned int slave_count)

   324

{

   325

  EtherCAT_command_t cmd;

   326

  EtherCAT_slave_t *cur;

   327

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

   328

  unsigned char data[2];

   329

   330

  // EtherCAT_clear_slaves() must be called before!

   331

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

   332

  {

   333

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

   334

    return -1;

   335

  }

   336

   337

  // No slaves.

   338

  if (slave_count == 0)

   339

  {

   340

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

   341

    return -1;

   342

  }

   343

   344

  // Determine number of slaves on bus

   345

   346

  EtherCAT_command_broadcast_read(&cmd, 0x0000, 4);

   347

   348

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

   349

   350

  if (cmd.working_counter != slave_count)

   351

  {

   352

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

   353

           cmd.working_counter, slave_count);

   354

    return -1;

   355

  }

   356

  else

   357

  {

   358

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

   359

  }

   360

   361

  // For every slave in the list

   362

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

   363

  {

   364

    cur = &slaves[i];

   365

   366

    if (!cur->desc)

   367

    {

   368

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

   369

      return -1;

   370

    }

   371

   372

    // Set ring position

   373

    cur->ring_position = -i;

   374

    cur->station_address = i + 1;

   375

   376

    // Write station address

   377

   378

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

   379

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

   380

   381

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

   382

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

   383

   384

    if (cmd.working_counter != 1)

   385

    {

   386

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

   387

      return -1;

   388

    }

   389

   390

    // Read base data

   391

   392

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

   393

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

   394

   395

    if (cmd.working_counter != 1)

   396

    {

   397

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

   398

      return -1;

   399

    }

   400

   401

    // Get base data

   402

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

   403

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

   404

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

   405

   406

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

   407

   408

    if (EtherCAT_read_slave_information(master, cur->station_address,

   409

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

   410

    {

   411

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

   412

      return -1;

   413

    }

   414

   415

    if (EtherCAT_read_slave_information(master, cur->station_address,

   416

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

   417

    {

   418

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

   419

      return -1;

   420

    }

   421

   422

    if (EtherCAT_read_slave_information(master, cur->station_address,

   423

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

   424

    {

   425

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

   426

      return -1;

   427

    }

   428

   429

    if (EtherCAT_read_slave_information(master, cur->station_address,

   430

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

   431

    {

   432

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

   433

      return -1;

   434

    }

   435

   436

    // Search for identification in "database"

   437

   438

    found = 0;

   439

   440

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

   441

    {

   442

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

   443

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

   444

      {

   445

        found = 1;

   446

   447

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

   448

        {

   449

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

   450

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

   451

                 slave_idents[j].desc->vendor_name,

   452

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

   453

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

   454

          return -1;

   455

        }

   456

   457

        break;

   458

      }

   459

    }

   460

   461

    if (!found)

   462

    {

   463

      EC_DBG(KERN_ERR "EtherCAT: Unknown slave device"

   464

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

   465

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

   466

      return -1;

   467

    }

   468

  }

   469

   470

  length = 0;

   471

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

   472

  {

   473

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

   474

  }

   475

   476

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

   477

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

   478

  {

   479

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

   480

    return -1;

   481

  }

   482

   483

  master->process_data_length = length;

   484

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

   485

   486

  pos = 0;

   487

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

   488

  {

   489

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

   490

    slaves[i].logical_address0 = pos;

   491

   492

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

   493

           i, pos, slaves[i].desc->vendor_name, slaves[i].desc->product_name,

   494

           slaves[i].serial_number);

   495

   496

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

   497

  }

   498

   499

  master->slaves = slaves;