1

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

     2

 *

     3

 *  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 "../include/ecrt.h"

    18

#include "globals.h"

    19

#include "master.h"

    20

#include "slave.h"

    21

#include "types.h"

    22

#include "device.h"

    23

#include "command.h"

    24

    25

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

    26

    27

/**

    28

   Konstruktor des EtherCAT-Masters.

    29

*/

    30

    31

void ec_master_init(ec_master_t *master /**< EtherCAT-Master */)

    32

{

    33

    master->slaves = NULL;

    34

    master->device = NULL;

    35

    36

    INIT_LIST_HEAD(&master->commands);

    37

    INIT_LIST_HEAD(&master->domains);

    38

    39

    ec_master_reset(master);

    40

}

    41

    42

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

    43

    44

/**

    45

   Destruktor eines EtherCAT-Masters.

    46

    47

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

    48

   auf das Slave-Array und gibt die Prozessdaten frei.

    49

*/

    50

    51

void ec_master_clear(ec_master_t *master /**< EtherCAT-Master */)

    52

{

    53

    ec_master_reset(master);

    54

    55

    if (master->device) {

    56

        ec_device_clear(master->device);

    57

        kfree(master->device);

    58

    }

    59

}

    60

    61

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

    62

    63

/**

    64

   Setzt den Master zurück in den Ausgangszustand.

    65

    66

   Bei einem "release" sollte immer diese Funktion aufgerufen werden,

    67

   da sonst Slave-Liste, Domains, etc. weiter existieren.

    68

*/

    69

    70

void ec_master_reset(ec_master_t *master

    71

                     /**< Zeiger auf den zurückzusetzenden Master */

    72

                     )

    73

{

    74

    unsigned int i;

    75

    ec_command_t *command, *next_command;

    76

    ec_domain_t *domain, *next_domain;

    77

    78

    // Alle Slaves entfernen

    79

    if (master->slaves) {

    80

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

    81

            ec_slave_clear(master->slaves + i);

    82

        }

    83

        kfree(master->slaves);

    84

        master->slaves = NULL;

    85

    }

    86

    master->slave_count = 0;

    87

    88

    // Kommando-Warteschlange leeren

    89

    list_for_each_entry_safe(command, next_command, &master->commands, list) {

    90

        command->state = EC_CMD_ERROR;

    91

        list_del_init(&command->list);

    92

    }

    93

    94

    // Domain-Liste leeren

    95

    list_for_each_entry_safe(domain, next_domain, &master->domains, list) {

    96

        list_del(&domain->list);

    97

        ec_domain_clear(domain);

    98

        kfree(domain);

    99

    }

   100

   101

    master->command_index = 0;

   102

    master->debug_level = 0;

   103

    master->timeout = 100; // us

   104

    master->stats.timeouts = 0;

   105

    master->stats.delayed = 0;

   106

    master->stats.corrupted = 0;

   107

    master->stats.unmatched = 0;

   108

    master->stats.t_last = 0;

   109

}

   110

   111

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

   112

   113

/**

   114

   Öffnet das EtherCAT-Geraet des Masters.

   115

   116

   \return 0 wenn alles ok, < 0 wenn kein Gerät registriert wurde oder

   117

           es nicht geoeffnet werden konnte.

   118

*/

   119

   120

int ec_master_open(ec_master_t *master /**< Der EtherCAT-Master */)

   121

{

   122

    if (!master->device) {

   123

        EC_ERR("No device registered!\n");

   124

        return -1;

   125

    }

   126

   127

    if (ec_device_open(master->device)) {

   128

        EC_ERR("Could not open device!\n");

   129

        return -1;

   130

    }

   131

   132

    return 0;

   133

}

   134

   135

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

   136

   137

/**

   138

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

   139

*/

   140

   141

void ec_master_close(ec_master_t *master /**< EtherCAT-Master */)

   142

{

   143

    if (!master->device) {

   144

        EC_WARN("Warning - Trying to close an unregistered device!\n");

   145

        return;

   146

    }

   147

   148

    if (ec_device_close(master->device))

   149

        EC_WARN("Warning - Could not close device!\n");

   150

}

   151

   152

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

   153

   154

/**

   155

   Stellt ein Kommando in die Warteschlange.

   156

*/

   157

   158

void ec_master_queue_command(ec_master_t *master, /**< EtherCAT-Master */

   159

                             ec_command_t *command /**< Kommando */

   160

                             )

   161

{

   162

    ec_command_t *queued_command;

   163

   164

    // Ist das Kommando schon in der Warteschlange?

   165

    list_for_each_entry(queued_command, &master->commands, list) {

   166

        if (queued_command == command) {

   167

            command->state = EC_CMD_QUEUED;

   168

            if (unlikely(master->debug_level))

   169

                EC_WARN("command already queued.\n");

   170

            return;

   171

        }

   172

    }

   173

   174

    list_add_tail(&command->list, &master->commands);

   175

    command->state = EC_CMD_QUEUED;

   176

}

   177

   178

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

   179

   180

/**

   181

   Sendet die Kommandos in der Warteschlange.

   182

   183

   \return 0 bei Erfolg, sonst < 0

   184

*/

   185

   186

void ec_master_send_commands(ec_master_t *master /**< EtherCAT-Master */)

   187

{

   188

    ec_command_t *command;

   189

    size_t command_size;

   190

    uint8_t *frame_data, *cur_data;

   191

    void *follows_word;

   192

    cycles_t start = 0, end;

   193

   194

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

   195

        EC_DBG("ec_master_send\n");

   196

        start = get_cycles();

   197

    }

   198

   199

    // Zeiger auf Socket-Buffer holen

   200

    frame_data = ec_device_tx_data(master->device);

   201

    cur_data = frame_data + EC_FRAME_HEADER_SIZE;

   202

    follows_word = NULL;

   203

   204

    // Aktuellen Frame mit Kommandos füllen

   205

    list_for_each_entry(command, &master->commands, list) {

   206

        if (command->state != EC_CMD_QUEUED) continue;

   207

   208

        // Passt das aktuelle Kommando noch in den aktuellen Rahmen?

   209

        command_size = EC_COMMAND_HEADER_SIZE + command->data_size

   210

            + EC_COMMAND_FOOTER_SIZE;

   211

        if (cur_data - frame_data + command_size > EC_MAX_FRAME_SIZE) break;

   212

   213

        command->state = EC_CMD_SENT;

   214

        command->index = master->command_index++;

   215

   216

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

   217

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

   218

   219

        // Command-Following-Flag im letzten Kommando setzen

   220

        if (follows_word)

   221

            EC_WRITE_U16(follows_word, EC_READ_U16(follows_word) | 0x8000);

   222

   223

        // EtherCAT command header

   224

        EC_WRITE_U8 (cur_data,     command->type);

   225

        EC_WRITE_U8 (cur_data + 1, command->index);

   226

        EC_WRITE_U32(cur_data + 2, command->address.logical);

   227

        EC_WRITE_U16(cur_data + 6, command->data_size & 0x7FF);

   228

        EC_WRITE_U16(cur_data + 8, 0x0000);

   229

        follows_word = cur_data + 6;

   230

        cur_data += EC_COMMAND_HEADER_SIZE;

   231

   232

        // EtherCAT command data

   233

        memcpy(cur_data, command->data, command->data_size);

   234

        cur_data += command->data_size;

   235

   236

        // EtherCAT command footer

   237

        EC_WRITE_U16(cur_data, command->working_counter);

   238

        cur_data += EC_COMMAND_FOOTER_SIZE;

   239

    }

   240

   241

    if (cur_data - frame_data == EC_FRAME_HEADER_SIZE) {

   242

        if (unlikely(master->debug_level > 0)) EC_DBG("nothing to send.\n");

   243

        return;

   244

    }

   245

   246

    // EtherCAT frame header

   247

    EC_WRITE_U16(frame_data, ((cur_data - frame_data

   248

                               - EC_FRAME_HEADER_SIZE) & 0x7FF) | 0x1000);

   249

   250

    // Rahmen auffüllen

   251

    while (cur_data - frame_data < EC_MIN_FRAME_SIZE)

   252

        EC_WRITE_U8(cur_data++, 0x00);

   253

   254

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

   255

        EC_DBG("Frame size: %i\n", cur_data - frame_data);

   256

   257

    // Send frame

   258

    ec_device_send(master->device, cur_data - frame_data);

   259

   260

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

   261

        end = get_cycles();

   262

        EC_DBG("ec_master_send finished in %ius.\n",

   263

               (u32) (end - start) * 1000 / cpu_khz);

   264

    }

   265

}

   266

   267

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

   268

   269

/**

   270

   Wertet einen empfangenen Rahmen aus.

   271

   272

   \return 0 bei Erfolg, sonst < 0

   273

*/

   274

   275

void ec_master_receive(ec_master_t *master, /**< EtherCAT-Master */

   276

                      const uint8_t *frame_data, /**< Empfangene Daten */

   277

                      size_t size /**< Anzahl empfangene Datenbytes */

   278

                      )

   279

{

   280

    size_t frame_size, data_size;

   281

    uint8_t command_type, command_index;

   282

    unsigned int cmd_follows, matched;

   283

    const uint8_t *cur_data;

   284

    ec_command_t *command;

   285

   286

    if (unlikely(size < EC_FRAME_HEADER_SIZE)) {

   287

        master->stats.corrupted++;

   288

        ec_master_output_stats(master);

   289

        return;

   290

    }

   291

   292

    cur_data = frame_data;

   293

   294

    // Länge des gesamten Frames prüfen

   295

    frame_size = EC_READ_U16(cur_data) & 0x07FF;

   296

    cur_data += EC_FRAME_HEADER_SIZE;

   297

   298

    if (unlikely(frame_size > size)) {

   299

        master->stats.corrupted++;

   300

        ec_master_output_stats(master);

   301

        return;

   302

    }

   303

   304

    cmd_follows = 1;

   305

    while (cmd_follows) {

   306

        // Kommando-Header auswerten

   307

        command_type  = EC_READ_U8 (cur_data);

   308

        command_index = EC_READ_U8 (cur_data + 1);

   309

        data_size     = EC_READ_U16(cur_data + 6) & 0x07FF;

   310

        cmd_follows   = EC_READ_U16(cur_data + 6) & 0x8000;

   311

        cur_data += EC_COMMAND_HEADER_SIZE;

   312

   313

        if (unlikely(cur_data - frame_data

   314

                     + data_size + EC_COMMAND_FOOTER_SIZE > size)) {

   315

            master->stats.corrupted++;

   316

            ec_master_output_stats(master);

   317

            return;

   318

        }

   319

   320

        // Suche passendes Kommando in der Liste

   321

        matched = 0;

   322

        list_for_each_entry(command, &master->commands, list) {

   323

            if (command->state == EC_CMD_SENT

   324

                && command->type == command_type

   325

                && command->index == command_index

   326

                && command->data_size == data_size) {

   327

                matched = 1;

   328

                break;

   329

            }

   330

        }

   331

   332

        // Kein passendes Kommando in der Liste gefunden

   333

        if (!matched) {

   334

            master->stats.unmatched++;

   335

            ec_master_output_stats(master);

   336

            cur_data += data_size + EC_COMMAND_FOOTER_SIZE;

   337

            continue;

   338

        }

   339

   340

        // Empfangene Daten in Kommando-Datenspeicher kopieren

   341

        memcpy(command->data, cur_data, data_size);

   342

        cur_data += data_size;

   343

   344

        // Working-Counter setzen

   345

        command->working_counter = EC_READ_U16(cur_data);

   346

        cur_data += EC_COMMAND_FOOTER_SIZE;

   347

   348

        // Kommando aus der Liste entfernen

   349

        command->state = EC_CMD_RECEIVED;

   350

        list_del_init(&command->list);

   351

    }

   352

}

   353

   354

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

   355

   356

/**

   357

   Sendet ein einzelnes Kommando und wartet auf den Empfang.

   358

   359

   Wenn der Slave nicht antwortet, wird das Kommando

   360

   nochmals gesendet.

   361

   362

   \return 0 bei Erfolg, sonst < 0

   363

*/

   364

   365

int ec_master_simple_io(ec_master_t *master, /**< EtherCAT-Master */

   366

                        ec_command_t *command /**< Kommando */

   367

                        )

   368

{

   369

    unsigned int response_tries_left;

   370

   371

    response_tries_left = 10;

   372

   373

    while (1)

   374

    {

   375

        ec_master_queue_command(master, command);

   376

        ecrt_master_sync_io(master);

   377

   378

        if (command->state == EC_CMD_RECEIVED) {

   379

            if (likely(command->working_counter))

   380

                return 0;

   381

        }

   382

        else if (command->state == EC_CMD_TIMEOUT) {

   383

            EC_ERR("Simple-IO TIMEOUT!\n");

   384

            return -1;

   385

        }

   386

        else if (command->state == EC_CMD_ERROR) {

   387

            EC_ERR("Simple-IO command error!\n");

   388

            return -1;

   389

        }

   390

   391

        // Keine direkte Antwort. Dem Slave Zeit lassen...

   392

        udelay(10);

   393

   394

        if (unlikely(--response_tries_left)) {

   395

            EC_ERR("No response in simple-IO!\n");

   396

            return -1;

   397

        }

   398

    }

   399

}

   400

   401

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

   402

   403

/**

   404

   Durchsucht den EtherCAT-Bus nach Slaves.

   405

   406

   Erstellt ein Array mit allen Slave-Informationen die für den

   407

   weiteren Betrieb notwendig sind.

   408

   409

   \return 0 bei Erfolg, sonst < 0

   410

*/

   411

   412

int ec_master_bus_scan(ec_master_t *master /**< EtherCAT-Master */)

   413

{

   414

    ec_command_t command;

   415

    ec_slave_t *slave;

   416

    ec_slave_ident_t *ident;

   417

    unsigned int i;

   418

    uint8_t data[2];

   419

   420

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

   421

        EC_ERR("Slave scan already done!\n");

   422

        return -1;

   423

    }

   424

   425

    // Determine number of slaves on bus

   426

    ec_command_init_brd(&command, 0x0000, 4);

   427

    if (unlikely(ec_master_simple_io(master, &command))) return -1;

   428

    master->slave_count = command.working_counter;

   429

    EC_INFO("Found %i slaves on bus.\n", master->slave_count);

   430

   431

    if (!master->slave_count) return 0;

   432

   433

    if (!(master->slaves = (ec_slave_t *) kmalloc(master->slave_count

   434

                                                  * sizeof(ec_slave_t),

   435

                                                  GFP_KERNEL))) {

   436

        EC_ERR("Could not allocate memory for slaves!\n");

   437

        return -1;

   438

    }

   439

   440

    // Init slaves

   441

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

   442

        slave = master->slaves + i;

   443

        ec_slave_init(slave, master);

   444

        slave->ring_position = i;

   445

        slave->station_address = i + 1;

   446

    }

   447

   448

    // For every slave in the list

   449

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

   450

    {

   451

        slave = master->slaves + i;

   452

   453

        // Write station address

   454

        EC_WRITE_U16(data, slave->station_address);

   455

        ec_command_init_apwr(&command, slave->ring_position,

   456

                             0x0010, sizeof(uint16_t), data);

   457

        if (unlikely(ec_master_simple_io(master, &command))) {

   458

            EC_ERR("Writing station address failed on slave %i!\n", i);

   459

            return -1;

   460

        }

   461

   462

        // Fetch all slave information

   463

        if (ec_slave_fetch(slave)) return -1;

   464

   465

        // Search for identification in "database"

   466

        ident = slave_idents;

   467

        while (ident) {

   468

            if (unlikely(ident->vendor_id == slave->sii_vendor_id

   469

                         && ident->product_code == slave->sii_product_code)) {

   470

                slave->type = ident->type;

   471

                break;

   472

            }

   473

            ident++;

   474

        }

   475

   476

        if (!slave->type)

   477

            EC_WARN("Unknown slave device (vendor 0x%08X, code 0x%08X) at"

   478

                    " position %i.\n", slave->sii_vendor_id,

   479

                    slave->sii_product_code, i);

   480

    }

   481

   482

    return 0;

   483

}

   484

   485

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

   486

   487

/**

   488

   Statistik-Ausgaben während des zyklischen Betriebs.

   489

   490

   Diese Funktion sorgt dafür, dass Statistiken während des zyklischen

   491

   Betriebs bei Bedarf, aber nicht zu oft ausgegeben werden.

   492