utils.py 8.0 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238
  1. #!/usr/bin/env python
  2. # -*- encoding: utf-8 -*-
  3. """
  4. Modbus TestKit: Implementation of Modbus protocol in python
  5. (C)2009 - Luc Jean - [email protected]
  6. (C)2009 - Apidev - http://www.apidev.fr
  7. This is distributed under GNU LGPL license, see license.txt
  8. """
  9. from __future__ import print_function
  10. import sys
  11. import threading
  12. import logging
  13. import socket
  14. import select
  15. from utils.modbus import LOGGER
  16. PY2 = sys.version_info[0] == 2
  17. PY3 = sys.version_info[0] == 3
  18. def threadsafe_function(fcn):
  19. """decorator making sure that the decorated function is thread safe"""
  20. lock = threading.RLock()
  21. def new(*args, **kwargs):
  22. """Lock and call the decorated function
  23. Unless kwargs['threadsafe'] == False
  24. """
  25. threadsafe = kwargs.pop('threadsafe', True)
  26. if threadsafe:
  27. lock.acquire()
  28. try:
  29. ret = fcn(*args, **kwargs)
  30. except Exception as excpt:
  31. raise excpt
  32. finally:
  33. if threadsafe:
  34. lock.release()
  35. return ret
  36. return new
  37. def flush_socket(socks, lim=0):
  38. """remove the data present on the socket"""
  39. input_socks = [socks]
  40. cnt = 0
  41. while True:
  42. i_socks = select.select(input_socks, input_socks, input_socks, 0.0)[0]
  43. if len(i_socks) == 0:
  44. break
  45. for sock in i_socks:
  46. sock.recv(1024)
  47. if lim > 0:
  48. cnt += 1
  49. if cnt >= lim:
  50. # avoid infinite loop due to loss of connection
  51. raise Exception("flush_socket: maximum number of iterations reached")
  52. def get_log_buffer(prefix, buff):
  53. """Format binary data into a string for debug purpose"""
  54. log = prefix
  55. for i in buff:
  56. log += str(ord(i) if PY2 else i) + "-"
  57. return log[:-1]
  58. class ConsoleHandler(logging.Handler):
  59. """This class is a logger handler. It prints on the console"""
  60. def __init__(self):
  61. """Constructor"""
  62. logging.Handler.__init__(self)
  63. def emit(self, record):
  64. """format and print the record on the console"""
  65. print(self.format(record))
  66. class LogitHandler(logging.Handler):
  67. """This class is a logger handler. It send to a udp socket"""
  68. def __init__(self, dest):
  69. """Constructor"""
  70. logging.Handler.__init__(self)
  71. self._dest = dest
  72. self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
  73. def emit(self, record):
  74. """format and send the record over udp"""
  75. data = self.format(record) + "\r\n"
  76. if PY3:
  77. data = to_data(data)
  78. self._sock.sendto(data, self._dest)
  79. class DummyHandler(logging.Handler):
  80. """This class is a logger handler. It doesn't do anything"""
  81. def __init__(self):
  82. """Constructor"""
  83. super(DummyHandler, self).__init__()
  84. def emit(self, record):
  85. """do nothing with the given record"""
  86. pass
  87. def create_logger(name="dummy", level=logging.DEBUG, record_format=None):
  88. """Create a logger according to the given settings"""
  89. if record_format is None:
  90. record_format = "%(asctime)s\t%(levelname)s\t%(module)s.%(funcName)s\t%(threadName)s\t%(message)s"
  91. logger = logging.getLogger("modbus_tk")
  92. logger.setLevel(level)
  93. formatter = logging.Formatter(record_format)
  94. if name == "udp":
  95. log_handler = LogitHandler(("127.0.0.1", 1975))
  96. elif name == "console":
  97. log_handler = ConsoleHandler()
  98. elif name == "dummy":
  99. log_handler = DummyHandler()
  100. else:
  101. raise Exception("Unknown handler %s" % name)
  102. log_handler.setFormatter(formatter)
  103. logger.addHandler(log_handler)
  104. return logger
  105. def swap_bytes(word_val):
  106. """swap lsb and msb of a word"""
  107. msb = (word_val >> 8) & 0xFF
  108. lsb = word_val & 0xFF
  109. return (lsb << 8) + msb
  110. def calculate_crc(data):
  111. """Calculate the CRC16 of a datagram"""
  112. CRC16table = (
  113. 0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241,
  114. 0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440,
  115. 0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40,
  116. 0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841,
  117. 0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40,
  118. 0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41,
  119. 0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641,
  120. 0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040,
  121. 0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240,
  122. 0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441,
  123. 0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41,
  124. 0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840,
  125. 0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41,
  126. 0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40,
  127. 0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640,
  128. 0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041,
  129. 0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240,
  130. 0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441,
  131. 0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41,
  132. 0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840,
  133. 0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41,
  134. 0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40,
  135. 0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640,
  136. 0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041,
  137. 0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241,
  138. 0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440,
  139. 0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40,
  140. 0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841,
  141. 0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40,
  142. 0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41,
  143. 0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641,
  144. 0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040
  145. )
  146. crc = 0xFFFF
  147. if PY2:
  148. for c in data:
  149. crc = (crc >> 8) ^ CRC16table[(ord(c) ^ crc) & 0xFF]
  150. else:
  151. for c in data:
  152. crc = (crc >> 8) ^ CRC16table[((c) ^ crc) & 0xFF]
  153. return swap_bytes(crc)
  154. def calculate_rtu_inter_char(baudrate):
  155. """calculates the interchar delay from the baudrate"""
  156. if baudrate <= 19200:
  157. return 11.0 / baudrate
  158. else:
  159. return 0.0005
  160. class WorkerThread(object):
  161. """
  162. A thread which is running an almost-ever loop
  163. It can be stopped by calling the stop function
  164. """
  165. def __init__(self, main_fct, args=(), init_fct=None, exit_fct=None):
  166. """Constructor"""
  167. self._fcts = [init_fct, main_fct, exit_fct]
  168. self._args = args
  169. self._thread = threading.Thread(target=WorkerThread._run, args=(self,))
  170. self._go = threading.Event()
  171. def start(self):
  172. """Start the thread"""
  173. self._go.set()
  174. self._thread.start()
  175. def stop(self):
  176. """stop the thread"""
  177. if self._thread.is_alive():
  178. self._go.clear()
  179. self._thread.join()
  180. def _run(self):
  181. """main function of the thread execute _main_fct until stop is called"""
  182. # pylint: disable=broad-except
  183. try:
  184. if self._fcts[0]:
  185. self._fcts[0](*self._args)
  186. while self._go.isSet():
  187. self._fcts[1](*self._args)
  188. except Exception as excpt:
  189. LOGGER.error("error: %s", str(excpt))
  190. finally:
  191. if self._fcts[2]:
  192. self._fcts[2](*self._args)
  193. def to_data(string_data):
  194. if PY2:
  195. return string_data
  196. else:
  197. return bytearray(string_data, 'ascii')