*** ble_driver.py	2019-12-20 13:35:37.000000000 -0000
--- ble_driver.py	2020-02-03 11:37:34.000000000 -0000
***************
*** 40,51 ****
--- 40,52 ----
  import subprocess
  import sys
  import time
  import traceback
  import queue
  import logging
+ import pyelliptic
  from abc import abstractmethod
  from threading import Thread, Lock
  
  from enum import Enum
  from typing import List
  
***************
*** 993,1004 ****
--- 994,1019 ----
      directed_advertiser_timeout = driver.BLE_HCI_DIRECTED_ADVERTISER_TIMEOUT
      conn_terminated_due_to_mic_failure = (
          driver.BLE_HCI_CONN_TERMINATED_DUE_TO_MIC_FAILURE
      )
      conn_failed_to_be_established = driver.BLE_HCI_CONN_FAILED_TO_BE_ESTABLISHED
  
+ class BLEGapLESCdhkey(object):
+     def __init__(self, key):
+         self.key = key
+ 
+     @classmethod
+     def from_c(cls, params):
+         return cls(key = util.uint8_array_to_list(params.key, 32))
+ 
+     def to_c(self):
+         params = driver.ble_gap_lesc_dhkey_t()
+         self.key_array = util.list_to_uint8_array(self.key)
+         params.key = self.key_array.cast()
+         return params
+ 
  
  class BLEUUIDBase(object):
      def __init__(self, vs_uuid_base=None, uuid_type=None):
          assert isinstance(vs_uuid_base, (list, type(None))), "Invalid argument type"
          assert isinstance(uuid_type, (int, type(None))), "Invalid argument type"
          if (vs_uuid_base is None) and uuid_type is None:
***************
*** 1035,1046 ****
--- 1050,1076 ----
      def to_c(self):
          lsb_list = self.base[::-1]
          self.__array = util.list_to_uint8_array(lsb_list)
          uuid = driver.ble_uuid128_t()
          uuid.uuid128 = self.__array.cast()
          return uuid
+ 
+ 
+ class BLEGapLESCp256pk(object):
+     def __init__(self, pk):
+         self.pk = pk
+ 
+     @classmethod
+     def from_c(cls, params):
+         return cls(pk = util.uint8_array_to_list(params.pk, 64))
+ 
+     def to_c(self):
+         params = driver.ble_gap_lesc_p256_pk_t()
+         self.pk_array = util.list_to_uint8_array(self.pk)
+         params.pk = self.pk_array.cast()
+         return params
  
  
  class BLEUUID(object):
      class Standard(Enum):
          unknown = 0x0000
          service_primary = 0x2800
***************
*** 1960,1989 ****
              sec_params, (BLEGapSecParams, type(None))
          ), "Invalid argument type"
          return driver.sd_ble_gap_authenticate(
              self.rpc_adapter, conn_handle, sec_params.to_c() if sec_params else None
          )
  
      @NordicSemiErrorCheck
      @wrapt.synchronized(api_lock)
      def ble_gap_sec_params_reply(
          self, conn_handle, sec_status, sec_params, own_keys=None, peer_keys=None
      ):
          assert isinstance(sec_status, BLEGapSecStatus), "Invalid argument type"
          assert isinstance(
              sec_params, (BLEGapSecParams, type(None))
          ), "Invalid argument type"
!         assert isinstance(own_keys, type(None)), "NOT IMPLEMENTED"
          assert isinstance(peer_keys, type(None)), "NOT IMPLEMENTED"
  
          keyset = driver.ble_gap_sec_keyset_t()
  
          keyset.keys_own.p_enc_key = driver.ble_gap_enc_key_t()
          keyset.keys_own.p_id_key = driver.ble_gap_id_key_t()
          keyset.keys_own.p_sign_key = driver.ble_gap_sign_info_t()
!         keyset.keys_own.p_pk = driver.ble_gap_lesc_p256_pk_t()
  
          keyset.keys_peer.p_enc_key = driver.ble_gap_enc_key_t()
          keyset.keys_peer.p_id_key = driver.ble_gap_id_key_t()
          keyset.keys_peer.p_sign_key = driver.ble_gap_sign_info_t()
          keyset.keys_peer.p_pk = driver.ble_gap_lesc_p256_pk_t()
  
--- 1990,2044 ----
              sec_params, (BLEGapSecParams, type(None))
          ), "Invalid argument type"
          return driver.sd_ble_gap_authenticate(
              self.rpc_adapter, conn_handle, sec_params.to_c() if sec_params else None
          )
  
+ 
+     def ecc_create_keys(self, curve='prime256v1'):
+         self.ecc = pyelliptic.ECC(curve=curve)
+         pk_str = self.ecc.get_pubkey(_format='hex')
+         print("\n\n" + pk_str)
+         pk_x = bytes.fromhex(pk_str[2:66])
+         pk_y = bytes.fromhex(pk_str[66:130])
+         return pk_x[::-1] + pk_y[::-1]
+ 
+     def ecc_get_dhkey(self, peer_pk):
+         peer_pk_x = peer_pk[0:32]
+         peer_pk_y = peer_pk[32:64]
+         peer_pk = peer_pk_x[::-1] + peer_pk_y[::-1]
+         peer_pk_str = "04" + bytes(peer_pk).hex()
+         print("\n\n1 " + peer_pk_str)
+         dhkey = self.ecc.get_ecdh_key(peer_pk_str, format='hex')
+         print("\n\n2 " + dhkey[::-1].hex())
+         return list(bytes.fromhex(dhkey[::-1].hex()))
+ 
+     def ble_gap_lesc_dhkey_reply(self, conn_handle, dhkey):
+         return driver.sd_ble_gap_lesc_dhkey_reply(self.rpc_adapter,
+                                                   conn_handle,
+                                                   dhkey.to_c())
+ 
+ 
      @NordicSemiErrorCheck
      @wrapt.synchronized(api_lock)
      def ble_gap_sec_params_reply(
          self, conn_handle, sec_status, sec_params, own_keys=None, peer_keys=None
      ):
          assert isinstance(sec_status, BLEGapSecStatus), "Invalid argument type"
          assert isinstance(
              sec_params, (BLEGapSecParams, type(None))
          ), "Invalid argument type"
!         #assert isinstance(own_keys, type(None)), "NOT IMPLEMENTED"
          assert isinstance(peer_keys, type(None)), "NOT IMPLEMENTED"
  
          keyset = driver.ble_gap_sec_keyset_t()
  
          keyset.keys_own.p_enc_key = driver.ble_gap_enc_key_t()
          keyset.keys_own.p_id_key = driver.ble_gap_id_key_t()
          keyset.keys_own.p_sign_key = driver.ble_gap_sign_info_t()
!         keyset.keys_own.p_pk = own_keys.to_c() if own_keys else driver.ble_gap_lesc_p256_pk_t()
  
          keyset.keys_peer.p_enc_key = driver.ble_gap_enc_key_t()
          keyset.keys_peer.p_id_key = driver.ble_gap_id_key_t()
          keyset.keys_peer.p_sign_key = driver.ble_gap_sign_info_t()
          keyset.keys_peer.p_pk = driver.ble_gap_lesc_p256_pk_t()
  
***************
*** 2312,2323 ****
--- 2367,2386 ----
                          ble_driver=self,
                          conn_handle=ble_event.evt.gap_evt.conn_handle,
                          peer_params=BLEGapSecParams.from_c(
                              sec_params_request_evt.peer_params
                          ),
                      )
+                     
+             elif evt_id == BLEEvtID.gap_evt_lesc_dhkey_request:
+                 lesc_dhkey_request_evt = ble_event.evt.gap_evt.params.lesc_dhkey_request
+ 
+                 for obs in self.observers:
+                     obs.on_gap_evt_lesc_dhkey_request(ble_driver  = self,
+                                                       conn_handle = ble_event.evt.gap_evt.conn_handle,
+                                                       p_pk_peer   = BLEGapLESCp256pk.from_c(lesc_dhkey_request_evt.p_pk_peer))
  
              elif evt_id == BLEEvtID.gap_evt_sec_info_request:
                  seq_info_evt = ble_event.evt.gap_evt.params.sec_info_request
  
                  for obs in self.observers:
                      obs.on_gap_evt_sec_info_request(
