1 import os,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 SDOProc = None

    12 Result = None

    13 

    14 def SDOThreadProc(*params):

    15     global Result, SDOProc

    16     if params[0] == "upload":

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

    18     else:

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

    20     

    21     command = cmdfmt % params[1:]

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

    23     res = SDOProc.wait()

    24     output = SDOProc.communicate()[0]

    25     

    26     if params[0] == "upload":

    27         Result = None

    28         if res == 0:

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

    30                 Result = float(output)

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

    32                 Result = output

    33             else:

    34                 hex_value, dec_value = output.split()

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

    36                     Result = int(dec_value)

    37     else:

    38         Result = res == 0

    39     

    40     SDOAnswered()

    41     if res != 0 :

    42         PLCObject.LogMessage(

    43             LogLevelsDict["WARNING"], 

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

    45     

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

    47     global SDOThread

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

    49     SDOThread.start()

    50     

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

    52     global SDOThread

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

    54     SDOThread.start()

    55 

    56 def GetResult():

    57     global Result

    58     return Result

    59 

    60 KMSGPollThread=None

    61 StopKMSGThread=False

    62 def KMSGPollThreadProc():

    63     """

    64     Logs Kernel messages starting with EtherCAT

    65     Uses GLibc wrapper to Linux syscall "klogctl"

    66     Last 4 KB are polled, and lines compared to last 

    67     captured line to detect new lines

    68     """

    69     global StopKMSGThread

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

    71     klog = libc.klogctl

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

    73     klog.restype = ctypes.c_int

    74     s=ctypes.create_string_buffer(4*1024)

    75     last = None

    76     while not StopKMSGThread:

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

    78         log = s.value[:l-1]

    79         if last :

    80             log = log.rpartition(last)[2]

    81         if log : 

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

    83             for lvl,msg in re.findall(

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

    85                             log, re.MULTILINE):

    86                 PLCObject.LogMessage(

    87                     LogLevelsDict[{

    88                         "4":"WARNING",

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

    90                     msg)

    91         time.sleep(0.5) 

    92 

    93 def _runtime_etherlab_init():

    94     global KMSGPollThread, StopKMSGThread

    95     StopKMSGThread = False

    96     KMSGPollThread = Thread(target = KMSGPollThreadProc)

    97     KMSGPollThread.start()

    98 

    99 def _runtime_etherlab_cleanup():

   100     global KMSGPollThread, StopKMSGThread, SDOProc, SDOThread

   101     try:

   102         os.kill(SDOProc.pid, SIGTERM)

   103     except:

   104         pass

   105     SDOThread = None

   106     StopKMSGThread = True

   107     KMSGPollThread = None

   108