1

import subprocess,sys,ctypes

     2

from threading import Thread

     3

import ctypes,time,re

     4

from targets.typemapping import LogLevelsDict

     5

     6

SDOAnswered = PLCBinary.SDOAnswered

     7

SDOAnswered.restype = None

     8

SDOAnswered.argtypes = []

     9

    10

SDOThread = None

    11

Result = None

    12

    13

def SDOThreadProc(*params):

    14

    global Result

    15

    if params[0] == "upload":

    16

        cmdfmt = "ethercat upload -p %d -t %s 0x%.4x 0x%.2x"

    17

    else:

    18

        cmdfmt = "ethercat download -p %d -t %s 0x%.4x 0x%.2x %s"

    19

    20

    command = cmdfmt % params[1:]

    21

    proc = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True)

    22

    res = proc.wait()

    23

    output = proc.communicate()[0]

    24

    25

    if params[0] == "upload":

    26

        Result = None

    27

        if res == 0:

    28

            if params[2] in ["float", "double"]:

    29

                Result = float(output)

    30

            elif params[2] in ["string", "octet_string", "unicode_string"]:

    31

                Result = output

    32

            else:

    33

                hex_value, dec_value = output.split()

    34

                if int(hex_value, 16) == int(dec_value):

    35

                    Result = int(dec_value)

    36

    else:

    37

        Result = res == 0

    38

    39

    SDOAnswered()

    40

    if res != 0 :

    41

        PLCObject.LogMessage(

    42

            LogLevelsDict["WARNING"], 

    43

            "%s : %s"%(command,output))

    44

    45

def EthercatSDOUpload(pos, index, subindex, var_type):

    46

    global SDOThread

    47

    SDOThread = Thread(target=SDOThreadProc, args=["upload", pos, var_type, index, subindex])

    48

    SDOThread.start()

    49

    50

def EthercatSDODownload(pos, index, subindex, var_type, value):

    51

    global SDOThread

    52

    SDOThread = Thread(target=SDOThreadProc, args=["download", pos, var_type, index, subindex, value])

    53

    SDOThread.start()

    54

    55

def GetResult():

    56

    global Result

    57

    return Result

    58

    59

KMSGPollThread=None

    60

StopKMSGThread=False

    61

def KMSGPollThreadProc():

    62

    """

    63

    Logs Kernel messages starting with EtherCAT

    64

    Uses GLibc wrapper to Linux syscall "klogctl"

    65

    Last 4 KB are polled, and lines compared to last 

    66

    captured line to detect new lines

    67

    """

    68

    global StopKMSGThread

    69

    libc=ctypes.CDLL("libc.so.6")

    70

    klog = libc.klogctl

    71

    klog.argtypes = [ctypes.c_int, ctypes.c_char_p, ctypes.c_int]

    72

    klog.restype = ctypes.c_int

    73

    s=ctypes.create_string_buffer(4*1024)

    74

    last = None

    75

    while not StopKMSGThread:

    76

        l = klog(3,s,len(s)-1)

    77

        log = s.value[:l-1]

    78

        if last :

    79

            log = log.rpartition(last)[2]

    80

        if log : 

    81

            last = log.rpartition('\n')[2]

    82

            for lvl,msg in re.findall(

    83

                            r'<(\d)>\[\s*\d*\.\d*\]\s*(EtherCAT\s*.*)$',

    84

                            log, re.MULTILINE):

    85

                PLCObject.LogMessage(

    86

                    LogLevelsDict[{

    87

                        "4":"WARNING",

    88

                        "3":"CRITICAL"}.get(lvl,"DEBUG")],

    89

                    msg)

    90

        time.sleep(0.5) 

    91

    92

def _runtime_etherlab_init():

    93

    global KMSGPollThread, StopKMSGThread

    94

    StopKMSGThread = False

    95

    KMSGPollThread = Thread(target = KMSGPollThreadProc)

    96

    KMSGPollThread.start()

    97

    98

def _runtime_etherlab_cleanup():

    99

    global KMSGPollThread, StopKMSGThread

   100

    StopKMSGThread = True

   101

    KMSGPollThread = None

   102