diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c
index fe8222b..8c58e95 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c
@@ -163,6 +163,28 @@ double can_connected() {
   return (double)getSocketCANConnectd();
 }
 
+double can_write(double canId,
+                 double len,
+                 double data0,
+                 double data1,
+                 double data2,
+                 double data3,
+                 double data4,
+                 double data5,
+                 double data6,
+                 double data7) {
+  return (double)writeSocketCAN(canId,
+                                len,
+                                data0,
+                                data1,
+                                data2,
+                                data3,
+                                data4,
+                                data5,
+                                data6,
+                                data7);
+}
+
 // Register data for plugin so ecmc know what to use
 struct ecmcPluginData pluginDataDef = {
   // Allways use ECMC_PLUG_VERSION_MAGIC
@@ -194,7 +216,7 @@ struct ecmcPluginData pluginDataDef = {
         // Function name (this is the name you use in ecmc plc-code)
         .funcName = "can_connect",
         // Function description
-        .funcDesc = "double can_connect() : Connect to can if (from config str).",
+        .funcDesc = "double can_connect() : Connect to can interface (from config str).",
         /**
         * 7 different prototypes allowed (only doubles since reg in plc).
         * Only funcArg${argCount} func shall be assigned the rest set to NULL.
@@ -217,7 +239,7 @@ struct ecmcPluginData pluginDataDef = {
         // Function name (this is the name you use in ecmc plc-code)
         .funcName = "can_connected",
         // Function description
-        .funcDesc = "double can_connected() : Connected to can if.",
+        .funcDesc = "double can_connected() : Connected to can interface.",
         /**
         * 7 different prototypes allowed (only doubles since reg in plc).
         * Only funcArg${argCount} func shall be assigned the rest set to NULL.
@@ -235,7 +257,31 @@ struct ecmcPluginData pluginDataDef = {
         .funcArg10 = NULL,
         .funcGenericObj = NULL,
       },
-  .funcs[2] = {0},  // last element set all to zero..
+  .funcs[2] =
+      { /*----can_connected----*/
+        // Function name (this is the name you use in ecmc plc-code)
+        .funcName = "can_write",
+        // Function description
+        .funcDesc = "double can_write(canId,len,data0..data7) : Write to can interface.",
+        /**
+        * 7 different prototypes allowed (only doubles since reg in plc).
+        * Only funcArg${argCount} func shall be assigned the rest set to NULL.
+        **/
+        .funcArg0 = NULL,
+        .funcArg1 = NULL,
+        .funcArg2 = NULL,
+        .funcArg3 = NULL,
+        .funcArg4 = NULL,
+        .funcArg5 = NULL,
+        .funcArg6 = NULL,
+        .funcArg7 = NULL,
+        .funcArg8 = NULL,
+        .funcArg9 = NULL,
+        .funcArg10 = can_write,
+        .funcGenericObj = NULL,
+      },
+
+  .funcs[3] = {0},  // last element set all to zero..
   // PLC consts
   .consts[0] = {0}, // last element set all to zero..
 };
diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp
index 2062dec..c248489 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp
@@ -207,7 +207,7 @@ void ecmcSocketCAN::doReadWorker() {
 
     if(cfgDbgMode_) {
       // Simulate candump printout
-      printf("\n0x%02X", rxmsg_.can_id);
+      printf("\nread 0x%02X", rxmsg_.can_id);
       printf(" [%d]", rxmsg_.can_dlc);
       for(int i=0; i<rxmsg_.can_dlc; i++ ) {
         printf(" 0x%02X", rxmsg_.data[i]);
@@ -229,16 +229,42 @@ void ecmcSocketCAN::doConnectWorker() {
   }
 }
 
-// Test can write function
-int ecmcSocketCAN::writeCAN() {
-  struct can_frame frame;
-	txmsg_.can_id  = 0x123;
-	txmsg_.can_dlc = 2;
-	txmsg_.data[0] = frame.data[0]+1;
-	txmsg_.data[1] = frame.data[1]+1;
+// Test can write function (simple if for plc func)
+void ecmcSocketCAN::writeCAN( uint32_t canId,
+                             uint8_t len,
+                             uint8_t data0,
+                             uint8_t data1,
+                             uint8_t data2,
+                             uint8_t data3,
+                             uint8_t data4,
+                             uint8_t data5,
+                             uint8_t data6,
+                             uint8_t data7) {
+	txmsg_.can_id  = canId;
+	txmsg_.can_dlc = len;
+	txmsg_.data[0] = data0;
+	txmsg_.data[1] = data1;
+  txmsg_.data[2] = data2;
+  txmsg_.data[3] = data3;
+  txmsg_.data[4] = data4;
+  txmsg_.data[5] = data5;
+  txmsg_.data[6] = data6;
+  txmsg_.data[7] = data7;
+
   // Maybe need to add the size to write here.. if struct is not full, hmm?!
 	int nbytes = write(socketId_, &txmsg_, sizeof(struct can_frame));
-	printf("\nWrote %d bytes\n", nbytes);
+  if (nbytes!=len) {
+    throw std::runtime_error( "Error in write.");
+  }
+
+  if(cfgDbgMode_) {	  
+    // Simulate candump printout
+    printf("\nwrite 0x%02X", txmsg_.can_id);
+    printf(" [%d]", txmsg_.can_dlc);
+    for(int i=0; i<txmsg_.can_dlc; i++ ) {
+      printf(" 0x%02X", txmsg_.data[i]);
+    }
+  }
 }
 
 void ecmcSocketCAN::initAsyn() {
diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h
index 381aebf..e3b3812 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h
@@ -56,7 +56,17 @@ class ecmcSocketCAN : public asynPortDriver {
   virtual asynStatus    readFloat64(asynUser *pasynUser, epicsFloat64 *value);
   void                  connectExternal();  
   int                   getConnected();
-  int                   writeCAN();   // Add args later
+  void                  writeCAN( uint32_t canId,
+                                  uint8_t len,
+                                  uint8_t data0,
+                                  uint8_t data1,
+                                  uint8_t data2,
+                                  uint8_t data3,
+                                  uint8_t data4,
+                                  uint8_t data5,
+                                  uint8_t data6,
+                                  uint8_t data7);
+
  private:
   void                  parseConfigStr(char *configStr);
   void                  initAsyn();
diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp
index f2506a2..822c7ec 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp
@@ -77,6 +77,38 @@ int getSocketCANConnectd() {
   return 0;
 }
 
+int writeSocketCAN( double canId,
+                    double len,
+                    double data0,
+                    double data1,
+                    double data2,
+                    double data3,
+                    double data4,
+                    double data5,
+                    double data6,
+                    double data7) {
+  if(can){
+    try {       
+      can->writeCAN((uint32_t) canId,
+                    (uint8_t) len,
+                    (uint8_t) data0,
+                    (uint8_t) data1,
+                    (uint8_t) data2,
+                    (uint8_t) data3,
+                    (uint8_t) data4,
+                    (uint8_t) data5,
+                    (uint8_t) data6,
+                    (uint8_t) data7);
+      return 0;
+    }
+    catch(std::exception& e) {
+      printf("Exception: %s.\n",e.what());
+      return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
+    }
+  }
+  return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
+}
+
 void deleteSocketCAN() {
   if(can) {
     delete (can);
diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.h
index e682a86..c34fbe4 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.h
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.h
@@ -39,11 +39,24 @@ int connectSocketCAN();
 
 int getSocketCANConnectd();
 
+
+/** \brief Write CAN
+ */
+int  writeSocketCAN( double canId,
+                     double len,
+                     double data0,
+                     double data1,
+                     double data2,
+                     double data3,
+                     double data4,
+                     double data5,
+                     double data6,
+                     double data7);
+
 /** \brief Delete SocketCAN object\n
  *
  * Should be called when destructs.\n
  */
-
 void deleteSocketCAN();
 
 # ifdef __cplusplus