M5StickC PlusでミニUDSサーバを実行（その３ ISO-TP編）

こちらの続きです

https://qiita.com/O_positive/items/c03971bd4d0d1ef3ef64

CAN通信は１フレームで最大８バイトしかデータを送れません。
大きなデータを送受信するために、トランスポート層プロトコルのISO-TPに対応し、データ分割やフロー制御を行えるようにします。

## 使用するライブラリ
こちらのiso-tpライブラリを使用します。

https://github.com/altelch/iso-tp

このライブラリはCAN通信にMCP_CAN_libを使用する前提となっており、私の環境ではそのままでは使えません。
ESP32CANで通信できるよう改造します。

## iso-tpライブラリの改造
作成したファイルは[こちら](#作成したファイル)
### １．元ファイルを取得
[iso-tpライブラリ](https://github.com/altelch/iso-tp)の```iso-tp.h```と```iso-tp.cpp```をコピーして以下のようにファイル名を変更して配置します。
```plantuml
@startsalt
{
{T
 + プロジェクト
 ++ src
 +++ iso-tp-ESP32CAN.h | iso-tpライブラリを改造
 +++ iso-tp-ESP32CAN.cpp | iso-tpライブラリを改造
 +++ main.cpp | 改造したiso-tpライブラリを使用
}
}
@endsalt
```

### ２．依存関係の解消
ベースのiso-tpライブラリは MCP_CAN_lib の使用が前提となっているので、関連する箇所を変更します。

<details><summary><font color="Brown">iso-tp-ESP32CAN.h の変更箇所</font></summary>

```diff_cpp:iso-tp-ESP32CAN.h の変更箇所
// mcp_can.hのインクルードを削除orコメントアウト
- #include <mcp_can.h>
+ // #include <mcp_can.h>

// INT32U型をuint32_tに変更（INT32Uを使用している箇所すべてを変更）
- INT32U tx_id=0;
+ uint32_t tx_id=0;
```
</details>

<details><summary><font color="Brown">iso-tp-ESP32CAN.cpp の変更箇所</font></summary>

```diff_cpp:iso-tp-ESP32CAN.cpp の変更箇所
// インクルードファイルの変更
- #include "iso-tp.h"
- #include <mcp_can.h>
- #include <mcp_can_dfs.h>
- #include <SPI.h>
+ #include "iso-tp-ESP32CAN.h"

// INT32U型をuint32_tに変更（INT32Uを使用している箇所すべてを変更）
- INT32U delta=millis()-wait_cf;
+ uint32_t delta=millis()-wait_cf;
```
</details>

### ３．CAN送受信関数の変更
CAN送受信関数として、ライブラリ外の関数を紐づけて使用できるように変更します。
<details open><summary><font color="Brown">送信処理のイメージ</font></summary>

```plantuml
@startuml
<style>
classDiagram {
    RoundCorner 15
    .mystyle {
        RoundCorner 5
        BackgroundColor cyan
        FontStyle Bold
        FontColor black
    }
}
</style>
class IsoTp.can_tx_func <<<mystyle>>>
class IsoTp.tx_func <<<mystyle>>>
hide circle
hide members
circle CANネットワーク
loop -right-> IsoTp.send : 大きなデータ
package IsoTp {
    send -down-> send_sf_など : フレーム単位の送信データ
    note right of send
        データ分割
    end note
    send_sf_など -down-> can_tx_func : 送信データ
    note bottom of IsoTp.can_tx_func #cyan
        **IsoTp::can_send**
        **から変更**
    end note
    can_tx_func -right- tx_func : 紐づけ
    note bottom of IsoTp.tx_func #cyan
        **set_can_tx_rx_ESP32CAN**
        **で紐づけ**
    end note

}
IsoTp.tx_func -right- can_tx_ESP32CAN : 紐づけ
can_tx_ESP32CAN -right-> ESP32CAN.CANWriteFrame : 送信データ
note bottom of can_tx_ESP32CAN
    前回作成した
    CAN送信用関数
end note
ESP32CAN -down-> CANネットワーク : 送信
@enduml
```
</details>
<details><summary><font color="Brown">受信処理のイメージ</font></summary>

```plantuml
@startuml
<style>
classDiagram {
    RoundCorner 15
    .mystyle {
        RoundCorner 5
        BackgroundColor cyan
        FontStyle Bold
        FontColor black
    }
}
</style>
class IsoTp.can_rx_func <<<mystyle>>>
class IsoTp.rx_func <<<mystyle>>>
hide circle
hide members
circle CANネットワーク
loop <-right- IsoTp.receive : 大きなデータ
package IsoTp {
    receive <-down- rcv_sf_など : フレーム単位の受信データ
    note right of receive
        データ結合
    end note
    rcv_sf_など <-down- can_rx_func : 受信データ
    note bottom of IsoTp.can_rx_func #cyan
        **IsoTp::can_receive**
        **から変更**
    end note
    can_rx_func -right- rx_func : 紐づけ
    note bottom of IsoTp.rx_func #cyan
        **set_can_tx_rx_ESP32CAN**
        **で紐づけ**
    end note
}
IsoTp.rx_func -right- can_rx_ESP32CAN : 紐づけ
note bottom of can_rx_ESP32CAN
    前回作成した
    CAN受信用関数
end note
can_rx_ESP32CAN <-right- ESP32CAN.CAN_cfg.rx_queue : 受信データ
ESP32CAN <-down- CANネットワーク: 受信
@enduml
```
</details>
<details><summary><font color="Brown">iso-tp-ESP32CAN.h の変更箇所</font></summary>

```diff_cpp:iso-tp-ESP32CAN.h の変更箇所
class IsoTp
{
  public:
-    IsoTp(MCP_CAN* bus, uint8_t mcp_int);
+    IsoTp();
+    void set_can_tx_rx_ESP32CAN(int (*tx_func)(uint32_t, uint8_t*, uint8_t),
+                                int (*rx_func)(uint32_t*, uint8_t*, uint8_t*));
  private:
-    MCP_CAN* _bus;
-    uint8_t  _mcp_int;
-    uint8_t  can_send(uint32_t id, uint8_t len, uint8_t *data);
-    uint8_t  can_receive(void);
+    int (*can_tx_func)(uint32_t, uint8_t*, uint8_t);
+    int (*can_rx_func)(uint32_t*, uint8_t*, uint8_t*);

```
</details>
<details><summary><font color="Brown">iso-tp-ESP32CAN.cpp の変更箇所</font></summary>

```diff_cpp:iso-tp-ESP32CAN.cpp の変更箇所
- IsoTp::IsoTp(MCP_CAN* bus, uint8_t mcp_int)
- {
-   _mcp_int = mcp_int;
-   _bus = bus;
- }
+ IsoTp::IsoTp() {
+     can_tx_funcN = nullptr;
+     can_rx_func = nullptr;
+ }
+ 
+ void IsoTp::set_can_tx_rx_ESP32CAN(int (*tx_func)(uint32_t, uint8_t*, uint8_t),
+                                    int (*rx_func)(uint32_t*, uint8_t*, uint8_t*)) {
+     can_tx_func = tx_func;
+     can_rx_func = rx_func;
+ }

// IsoTp::can_send および IsoTp::can_receive を削除orコメントアウト
- uint8_t IsoTp::can_send(uint32_t id, uint8_t len, uint8_t *data)
- {
- }
-
- uint8_t IsoTp::can_receive(void)
- {
- }
```
</details>

### ４．CAN送受信関数の呼び出し箇所の変更
<details><summary><font color="Brown">iso-tp-ESP32CAN.cpp のCAN送信箇所の変更</font></summary>

```diff_cpp:iso-tp-ESP32CAN.cpp の変更箇所
// 送信箇所（複数個所すべて）
- return can_send(msg->tx_id,8,TxBuf);
+ return can_tx_func(msg->tx_id, TxBuf, 8);
```
</details>
<details><summary><font color="Brown">iso-tp-ESP32CAN.cpp のIsoTp::sendの中のCAN受信箇所の変更</font></summary>

```diff_cpp:iso-tp-ESP32CAN.cpp の変更箇所
// IsoTp::sendの中の受信箇所
- if(can_receive())
+ uint8_t data[8];
+ uint8_t len;
+ uint32_t id;
+ if (can_rx_func(&id, data, &len) == 0)
  {
-   if(rxId==msg->rx_id)
+   if(id==msg->rx_id)
    {
      retval=rcv_fc(msg);
      memset(rxBuffer,0,sizeof(rxBuffer));
    }
  }

```
</details>
<details><summary><font color="Brown">iso-tp-ESP32CAN.cpp のIsoTp::receiveの中のCAN受信箇所の変更</font></summary>

```diff_cpp:iso-tp-ESP32CAN.cpp の変更箇所
// IsoTp::receiveの中の受信箇所
- if(can_receive())
+ uint8_t data[8];
+ uint8_t len;
+ uint32_t id;
+ if (can_rx_func(&id, data, &len) == 0)
  {
-   if(rxId==msg->rx_id)
+   if(id==msg->rx_id)
    {
+     memcpy(rxBuffer, data, len); // rxBuffer にデータをコピー
      n_pci_type=rxBuffer[0] & 0xF0;
      switch (n_pci_type)
      {
        case N_PCI_FC:
        case N_PCI_SF:
        case N_PCI_FF:
        case N_PCI_CF:
      }
      memset(rxBuffer,0,sizeof(rxBuffer));
    }
  }
```
</details>

### ５．main.cppを記述
main.cppを[こちら](#作成したファイル)の「作成したファイル」のように記述します。


**ポイント**
setup関数の中の以下の記述でIsoTpのCAN送受信関数とESP32CAN用のCAN送受信関数を紐づける
```cpp
// ISO-TP の初期化（ESP32CAN 用の送受信関数を登録）
isotp.set_can_tx_rx_ESP32CAN(can_tx_ESP32CAN, can_rx_ESP32CAN);
```
### ６．送信の確認
:::note alert
ラズパイ側のCANを起動しておかないと送信されません。
ラズパイ側の環境設定は以下をご参照ください。
[車両診断通信プロトコル：UDSをラズパイでシミュレート](https://qiita.com/O_positive/items/637cfdbcb8b124c0163f)
:::
platformIOのプロジェクトをビルドしてM5StickC Plusにアップロードすると、９バイトのデータ（```0x11 0x22 ･･･ 0x99```）を１秒間隔で繰り返し送信します。
ラズパイ側でcandumpして確認すると、以下のようにデータ分割して、定められたフロー制御に従って送信されていることが確認できます。
```console
# candump can0
  can0  123   [8]  10 09 11 22 33 44 55 66
  can0  456   [8]  30 08 20 00 00 00 00 00
  can0  123   [8]  21 77 88 99 00 00 00 00
```
```plantuml
@startuml
participant M5StickC_Plus
M5StickC_Plus -> ラズパイ: FF
ラズパイ -> M5StickC_Plus: FC
M5StickC_Plus -> ラズパイ: CF
@enduml
```

### ７．受信の確認
ラズパイ側から２２バイトのデータ（```0x11 0x01 0x11 0x22 ･･･```）を送信し、
ラズパイ側でcandumpして確認すると、以下のように定められたフロー制御に従って受信していることが確認できます。
```console
  can0  456   [8]  10 16 11 01 11 22 33 44
  can0  123   [8]  30 00 00 00 00 00 00 00
  can0  456   [8]  21 55 66 77 88 99 00 11
  can0  456   [8]  22 22 33 44 55 66 77 88
  can0  456   [8]  23 99 00 00 00 00 00 00
```
```plantuml
@startuml
participant M5StickC_Plus
ラズパイ -> M5StickC_Plus: FF
M5StickC_Plus -> ラズパイ: FC
ラズパイ -> M5StickC_Plus: CF
ラズパイ -> M5StickC_Plus: CF
ラズパイ -> M5StickC_Plus: CF
@enduml
```

### 作成したファイル
<details><summary><font color="Brown">main.cpp</font></summary>

```c:main.cpp
#include "M5StickCPlus.h"
#include "ESP32CAN.h"
#include "CAN_config.h"
#include "iso-tp-ESP32CAN.h"

// ========================
// GPIO 定義
// ========================
#define TX_GPIO GPIO_NUM_32  // TXピン
#define RX_GPIO GPIO_NUM_33  // RXピン

// ========================
// グローバル変数
// ========================
CAN_device_t CAN_cfg;  // CAN設定構造体
IsoTp isotp;
struct Message_t txMsg, rxMsg;

// ========================
// CAN 初期化関数
// ========================
void initCAN() {
    CAN_cfg.speed = CAN_SPEED_500KBPS;
    CAN_cfg.tx_pin_id = TX_GPIO;
    CAN_cfg.rx_pin_id = RX_GPIO;
    CAN_cfg.rx_queue = xQueueCreate(10, sizeof(CAN_frame_t));  // 受信キュー

    if (ESP32Can.CANInit() == ESP_OK) {
        Serial.println("CAN bus started successfully!");
    } else {
        Serial.println("CAN bus failed to start!");
        while (1);  // 停止
    }
}

// ========================
// CAN 送信関数（ESP32CAN 用）
// ========================
int can_tx_ESP32CAN(uint32_t id, uint8_t *data, uint8_t len) {
    CAN_frame_t txFrame = { 0 };
    txFrame.FIR.B.FF = CAN_frame_std;
    txFrame.MsgID = id;
    txFrame.FIR.B.DLC = (len > 8) ? 8 : len;
    memcpy(txFrame.data.u8, data, txFrame.FIR.B.DLC);

    Serial.print("送信データ: ");
    for (int i = 0; i < txFrame.FIR.B.DLC; i++) {
        Serial.printf("%02X ", txFrame.data.u8[i]);
    }

    if (ESP32Can.CANWriteFrame(&txFrame) == ESP_OK) {
        Serial.println("-> 送信成功");
        return 0;
    } else {
        Serial.println("-> 送信エラー");
        return -1;
    }
}

// ========================
// CAN 受信関数（ESP32CAN 用）
// ========================
int can_rx_ESP32CAN(uint32_t *id, uint8_t *data, uint8_t *len) {
    CAN_frame_t rxFrame = { 0 };

    if (xQueueReceive(CAN_cfg.rx_queue, &rxFrame, pdMS_TO_TICKS(100)) == pdTRUE) {
        *id = rxFrame.MsgID;
        *len = rxFrame.FIR.B.DLC;
        memcpy(data, rxFrame.data.u8, *len);
        return 0;
    }

    return -1;  // 受信失敗
}

// ========================
// Arduino 初期化関数
// ========================
void setup() {
    M5.begin();
    Serial.begin(115200);
    initCAN();

    // ISO-TP の初期化（ESP32CAN 用の送受信関数を登録）
    isotp.set_can_tx_rx_ESP32CAN(can_tx_ESP32CAN, can_rx_ESP32CAN);
    txMsg.Buffer = (uint8_t *)calloc(MAX_MSGBUF, sizeof(uint8_t));
    rxMsg.Buffer = (uint8_t *)calloc(MAX_MSGBUF, sizeof(uint8_t));
}

// ========================
// メインループ
// ========================
void loop() {
    // 送信
    uint8_t tx_data[] = {0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99};
    txMsg.len = sizeof(tx_data);
    txMsg.tx_id = 0x123;
    txMsg.rx_id = 0x456;
    memcpy(txMsg.Buffer,tx_data,sizeof(tx_data));
    isotp.send(&txMsg);

    // // 受信
    rxMsg.tx_id = 0x123;
    rxMsg.rx_id = 0x456;
    isotp.receive(&rxMsg);
    isotp.print_buffer(rxMsg.rx_id, rxMsg.Buffer, rxMsg.len);

    delay(1000);  // 周期調整
}
```
</details>
<details><summary><font color="Brown">iso-tp-ESP32CAN.h</font></summary>

```c:iso-tp-ESP32CAN.h
#ifndef _ISOTP_H
#define _ISOTP_H

// #include <mcp_can.h>

// #define ISO_TP_DEBUG

typedef enum {
  ISOTP_IDLE = 0,
  ISOTP_SEND,
  ISOTP_SEND_FF,
  ISOTP_SEND_CF,
  ISOTP_WAIT_FIRST_FC,
  ISOTP_WAIT_FC,
  ISOTP_WAIT_DATA,
  ISOTP_FINISHED,
  ISOTP_ERROR
} isotp_states_t;

#define CAN_MAX_DLEN 8  //Not extended CAN

/* N_PCI type values in bits 7-4 of N_PCI bytes */
#define N_PCI_SF  0x00  /* single frame */
#define N_PCI_FF  0x10  /* first frame */
#define N_PCI_CF  0x20  /* consecutive frame */
#define N_PCI_FC  0x30  /* flow control */

#define FC_CONTENT_SZ 3 /* flow control content size in byte (FS/BS/STmin) */

/* Flow Status given in FC frame */
#define ISOTP_FC_CTS  0   /* clear to send */
#define ISOTP_FC_WT 1     /* wait */
#define ISOTP_FC_OVFLW  2 /* overflow */

/* Timeout values */
#define TIMEOUT_SESSION  500 /* Timeout between successfull send and receive */
#define TIMEOUT_FC       250 /* Timeout between FF and FC or Block CF and FC */
#define TIMEOUT_CF       250 /* Timeout between CFs                          */
#define MAX_FCWAIT_FRAME  10

#define MAX_MSGBUF 128    /* Received Message Buffer. Depends on uC ressources!
                             Should be enough for our needs */
struct Message_t
{
  uint16_t len=0;
  isotp_states_t tp_state=ISOTP_IDLE;
  uint16_t seq_id=1;
  uint8_t fc_status=ISOTP_FC_CTS;
  uint8_t blocksize=0;
  uint8_t min_sep_time=0;
  uint32_t tx_id=0;
  uint32_t rx_id=0;
  uint8_t *Buffer;
};

class IsoTp
{
  public:
    // IsoTp(MCP_CAN* bus, uint8_t mcp_int);
    IsoTp();
    void set_can_tx_rx_ESP32CAN(int (*tx_func)(uint32_t, uint8_t*, uint8_t),
                                int (*rx_func)(uint32_t*, uint8_t*, uint8_t*));
    uint8_t send(Message_t* msg);
    uint8_t receive(Message_t* msg);
    void    print_buffer(uint32_t id, uint8_t *buffer, uint16_t len);
  private:
    // MCP_CAN* _bus;
    // uint8_t  _mcp_int;
    int (*can_tx_func)(uint32_t, uint8_t*, uint8_t);
    int (*can_rx_func)(uint32_t*, uint8_t*, uint8_t*);
    uint32_t   rxId;
    uint8_t  rxLen;
    uint8_t  rxBuffer[8];
    uint16_t rest;
    uint8_t  fc_wait_frames=0;
    uint32_t   wait_fc=0;
    uint32_t   wait_cf=0;
    uint32_t   wait_session=0;
    // uint8_t  can_send(uint32_t id, uint8_t len, uint8_t *data);
    // uint8_t  can_receive(void);
    uint8_t  send_fc(struct Message_t* msg);
    uint8_t  send_sf(struct Message_t* msg);
    uint8_t  send_ff(struct Message_t* msg);
    uint8_t  send_cf(struct Message_t* msg);
    uint8_t  rcv_sf(struct Message_t* msg);
    uint8_t  rcv_ff(struct Message_t* msg);
    uint8_t  rcv_cf(struct Message_t* msg);
    uint8_t  rcv_fc(struct Message_t* msg);
    void     fc_delay(uint8_t sep_time);
};

#endif

```
</details>
<details><summary><font color="Brown">iso-tp-ESP32CAN.cpp</font></summary>

```c:iso-tp-ESP32CAN.cpp
#include "Arduino.h"
// #include "iso-tp.h"
// #include <mcp_can.h>
// #include <mcp_can_dfs.h>
// #include <SPI.h>
#include "iso-tp-ESP32CAN.h"

// IsoTp::IsoTp(MCP_CAN* bus, uint8_t mcp_int)
// {
//   _mcp_int = mcp_int;
//   _bus = bus;
// }

IsoTp::IsoTp() {
    can_tx_func = nullptr;
    can_rx_func = nullptr;
}
 
void IsoTp::set_can_tx_rx_ESP32CAN(int (*tx_func)(uint32_t, uint8_t*, uint8_t),
                                   int (*rx_func)(uint32_t*, uint8_t*, uint8_t*)) {
    can_tx_func = tx_func;
    can_rx_func = rx_func;
}

void IsoTp::print_buffer(uint32_t id, uint8_t *buffer, uint16_t len)
{
  uint16_t i=0;

  Serial.print(F("Buffer: "));
  Serial.print(id,HEX); Serial.print(F(" ["));
  Serial.print(len); Serial.print(F("] "));
  for(i=0;i<len;i++)
  {
    if(buffer[i] < 0x10) Serial.print(F("0"));
    Serial.print(buffer[i],HEX);
    Serial.print(F(" "));
  }
  Serial.println();
}

// uint8_t IsoTp::can_send(uint32_t id, uint8_t len, uint8_t *data)
// {
// #ifdef ISO_TP_DEBUG
//   Serial.println(F("Send CAN RAW Data:"));
//   print_buffer(id, data, len);
// #endif
//   return _bus->sendMsgBuf(id, 0, len, data);
// }

// uint8_t IsoTp::can_receive(void)
// {
//   bool msgReceived;

//   if (_mcp_int)
//     msgReceived = (!digitalRead(_mcp_int));                     // IRQ: if pin is low, read receive buffer
//   else
//     msgReceived = (_bus->checkReceive() == CAN_MSGAVAIL);       // No IRQ: poll receive buffer

//   if (msgReceived)
//   {
//      memset(rxBuffer,0,sizeof(rxBuffer));       // Cleanup Buffer
//      _bus->readMsgBuf(&rxId, &rxLen, rxBuffer); // Read data: buf = data byte(s)
// #ifdef ISO_TP_DEBUG
//      Serial.println(F("Received CAN RAW Data:"));
//      print_buffer(rxId, rxBuffer, rxLen);
// #endif
//     return true;
//   }
//   else return false;
// }

uint8_t IsoTp::send_fc(struct Message_t *msg)
{
  uint8_t TxBuf[8]={0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
  // FC message high nibble = 0x3 , low nibble = FC Status
  TxBuf[0]=(N_PCI_FC | msg->fc_status);
  TxBuf[1]=msg->blocksize;
  /* fix wrong separation time values according spec */
  if ((msg->min_sep_time > 0x7F) && ((msg->min_sep_time < 0xF1)
      || (msg->min_sep_time > 0xF9))) msg->min_sep_time = 0x7F;
  TxBuf[2]=msg->min_sep_time;
  // return can_send(msg->tx_id,8,TxBuf);
  return can_tx_func(msg->tx_id, TxBuf, 8);
}

uint8_t IsoTp::send_sf(struct Message_t *msg) //Send SF Message
{
  uint8_t TxBuf[8]={0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
  // SF message high nibble = 0x0 , low nibble = Length
  TxBuf[0]=(N_PCI_SF | msg->len);
  memcpy(TxBuf+1,msg->Buffer,msg->len);
//  return can_send(msg->tx_id,msg->len+1,TxBuf);// Add PCI length
  // return can_send(msg->tx_id,8,TxBuf);// Always send full frame
  return can_tx_func(msg->tx_id, TxBuf, 8);
}

uint8_t IsoTp::send_ff(struct Message_t *msg) // Send FF
{
  uint8_t TxBuf[8]={0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
  msg->seq_id=1;

  TxBuf[0]=(N_PCI_FF | ((msg->len&0x0F00) >> 8));
  TxBuf[1]=(msg->len&0x00FF);
  memcpy(TxBuf+2,msg->Buffer,6);             // Skip 2 Bytes PCI
  // return can_send(msg->tx_id,8,TxBuf);       // First Frame has full length
  return can_tx_func(msg->tx_id, TxBuf, 8);
}

uint8_t IsoTp::send_cf(struct Message_t *msg) // Send SF Message
{
  uint8_t TxBuf[8]={0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
  uint16_t len=7;

  TxBuf[0]=(N_PCI_CF | (msg->seq_id & 0x0F));
  if(msg->len>7) len=7; else len=msg->len;
  memcpy(TxBuf+1,msg->Buffer,len);         // Skip 1 Byte PCI
  //return can_send(msg->tx_id,len+1,TxBuf); // Last frame is probably shorter
                                           // than 8 -> Signals last CF Frame
  // return can_send(msg->tx_id,8,TxBuf);     // Last frame is probably shorter
  return can_tx_func(msg->tx_id, TxBuf, 8);
                                           // than 8, pad with 00
}

void IsoTp::fc_delay(uint8_t sep_time)
{
  /*
  * 0x00 - 0x7F: 0 - 127ms
  * 0x80 - 0xF0: reserved
  * 0xF1 - 0xF9: 100us - 900us
  * 0xFA - 0xFF: reserved
  * default 0x7F, 127ms
  */
  if(sep_time <= 0x7F)
    delay(sep_time);
  else if ((sep_time >= 0xF1) && (sep_time <= 0xF9))
    delayMicroseconds((sep_time-0xF0)*100);
  else
    delay(0x7F);
}

uint8_t IsoTp::rcv_sf(struct Message_t* msg)
{
  /* get the SF_DL from the N_PCI byte */
  msg->len = rxBuffer[0] & 0x0F;
  /* copy the received data bytes */
  memcpy(msg->Buffer,rxBuffer+1,msg->len); // Skip PCI, SF uses len bytes
  msg->tp_state=ISOTP_FINISHED;

  return 0;
}

uint8_t IsoTp::rcv_ff(struct Message_t* msg)
{
  msg->seq_id=1;

  /* get the FF_DL */
  msg->len = (rxBuffer[0] & 0x0F) << 8;
  msg->len += rxBuffer[1];
  rest=msg->len;

  /* copy the first received data bytes */
  memcpy(msg->Buffer,rxBuffer+2,6); // Skip 2 bytes PCI, FF must have 6 bytes!
  rest-=6; // Restlength

  msg->tp_state = ISOTP_WAIT_DATA;

#ifdef ISO_TP_DEBUG
  Serial.print(F("First frame received with message length: "));
  Serial.println(rest);
  Serial.println(F("Send flow controll."));
  Serial.print(F("ISO-TP state: ")); Serial.println(msg->tp_state);
#endif

  /* send our first FC frame with Target Address*/
  struct Message_t fc;
  fc.tx_id=msg->tx_id;
  fc.fc_status=ISOTP_FC_CTS;
  fc.blocksize=0;
  fc.min_sep_time=0;
  return send_fc(&fc);
}

uint8_t IsoTp::rcv_cf(struct Message_t* msg)
{
  //Handle Timeout
  //If no Frame within 250ms change State to ISOTP_IDLE
  uint32_t delta=millis()-wait_cf;

  if((delta >= TIMEOUT_FC) && msg->seq_id>1)
  {
#ifdef ISO_TP_DEBUG
    Serial.println(F("CF frame timeout during receive wait_cf="));
    Serial.print(wait_cf); Serial.print(F(" delta="));
    Serial.println(delta);
#endif
    msg->tp_state = ISOTP_IDLE;
    return 1;
  }
  wait_cf=millis();

#ifdef ISO_TP_DEBUG
  Serial.print(F("ISO-TP state: ")); Serial.println(msg->tp_state);
  Serial.print(F("CF received with message rest length: "));
  Serial.println(rest);
#endif

  if (msg->tp_state != ISOTP_WAIT_DATA) return 0;

  if ((rxBuffer[0] & 0x0F) != (msg->seq_id & 0x0F))
  {
#ifdef ISO_TP_DEBUG
    Serial.print(F("Got sequence ID: ")); Serial.print(rxBuffer[0] & 0x0F);
    Serial.print(F(" Expected: ")); Serial.println(msg->seq_id & 0x0F);
#endif
    msg->tp_state = ISOTP_IDLE;
    msg->seq_id = 1;
    return 1;
  }

  if(rest<=7) // Last Frame
  {
    memcpy(msg->Buffer+6+7*(msg->seq_id-1),rxBuffer+1,rest);// 6 Bytes in FF +7
    msg->tp_state=ISOTP_FINISHED;                           // per CF skip PCI
#ifdef ISO_TP_DEBUG
    Serial.print(F("Last CF received with seq. ID: "));
    Serial.println(msg->seq_id);
#endif
  }
  else
  {
#ifdef ISO_TP_DEBUG
    Serial.print(F("CF received with seq. ID: "));
    Serial.println(msg->seq_id);
#endif
    memcpy(msg->Buffer+6+7*(msg->seq_id-1),rxBuffer+1,7); // 6 Bytes in FF +7
                                                          // per CF
    rest-=7; // Got another 7 Bytes of Data;
  }

  msg->seq_id++;

  return 0;
}

uint8_t IsoTp::rcv_fc(struct Message_t* msg)
{
  uint8_t retval=0;

  if (msg->tp_state != ISOTP_WAIT_FC && msg->tp_state != ISOTP_WAIT_FIRST_FC)
    return 0;

  /* get communication parameters only from the first FC frame */
  if (msg->tp_state == ISOTP_WAIT_FIRST_FC)
  {
    msg->blocksize = rxBuffer[1];
    msg->min_sep_time = rxBuffer[2];

    /* fix wrong separation time values according spec */
    if ((msg->min_sep_time > 0x7F) && ((msg->min_sep_time < 0xF1)
	|| (msg->min_sep_time > 0xF9))) msg->min_sep_time = 0x7F;
  }

#ifdef ISO_TP_DEBUG
  Serial.print(F("FC frame: FS "));
  Serial.print(rxBuffer[0]&0x0F);
  Serial.print(F(", Blocksize "));
  Serial.print(msg->blocksize);
  Serial.print(F(", Min. separation Time "));
  Serial.println(msg->min_sep_time);
#endif

  switch (rxBuffer[0] & 0x0F)
  {
    case ISOTP_FC_CTS:
                         msg->tp_state = ISOTP_SEND_CF;
                         break;
    case ISOTP_FC_WT:
                         fc_wait_frames++;
			 if(fc_wait_frames >= MAX_FCWAIT_FRAME)
                         {
#ifdef ISO_TP_DEBUG
                           Serial.println(F("FC wait frames exceeded."));
#endif
                           fc_wait_frames=0;
                           msg->tp_state = ISOTP_IDLE;
                           retval=1;
                         }
#ifdef ISO_TP_DEBUG
                         Serial.println(F("Start waiting for next FC"));
#endif
                         break;
    case ISOTP_FC_OVFLW:
#ifdef ISO_TP_DEBUG
                         Serial.println(F("Overflow in receiver side"));
#endif
    default:
                         msg->tp_state = ISOTP_IDLE;
                         retval=1;
  }
  return retval;
}

uint8_t IsoTp::send(Message_t* msg)
{
  uint8_t bs=false;
  uint32_t delta=0;
  uint8_t retval=0;

  msg->tp_state=ISOTP_SEND;

  while(msg->tp_state!=ISOTP_IDLE && msg->tp_state!=ISOTP_ERROR)
  {
    bs=false;

#ifdef ISO_TP_DEBUG
    Serial.print(F("ISO-TP State: ")); Serial.println(msg->tp_state);
    Serial.print(F("Length      : ")); Serial.println(msg->len);
#endif

    switch(msg->tp_state)
    {
      case ISOTP_IDLE         :  break;
      case ISOTP_SEND         :
                                 if(msg->len<=7)
                                 {
#ifdef ISO_TP_DEBUG
                                   Serial.println(F("Send SF"));
#endif
                                   retval=send_sf(msg);
                                   msg->tp_state=ISOTP_IDLE;
                                 }
                                 else
                                 {
#ifdef ISO_TP_DEBUG
                                   Serial.println(F("Send FF"));
#endif
                                   if(!(retval=send_ff(msg))) // FF complete
                                   {
                                     msg->Buffer+=6;
                                     msg->len-=6;
                                     msg->tp_state=ISOTP_WAIT_FIRST_FC;
                                     fc_wait_frames=0;
                                     wait_fc=millis();
                                   }
                                 }
                                 break;
      case ISOTP_WAIT_FIRST_FC:
#ifdef ISO_TP_DEBUG
                                 Serial.println(F("Wait first FC"));
#endif
                                 delta=millis()-wait_fc;
                                 if(delta >= TIMEOUT_FC)
                                 {
#ifdef ISO_TP_DEBUG
                                   Serial.print(F("FC timeout during receive"));
                                   Serial.print(F(" wait_fc="));
                                   Serial.print(wait_fc);
                                   Serial.print(F(" delta="));
                                   Serial.println(delta);
#endif
                                   msg->tp_state = ISOTP_IDLE;
				   retval=1;
                                 }
                                 break;
      case ISOTP_WAIT_FC      :
#ifdef ISO_TP_DEBUG
                                 Serial.println(F("Wait FC"));
#endif
                                 break;
      case ISOTP_SEND_CF      :
#ifdef ISO_TP_DEBUG
                                 Serial.println(F("Send CF"));
#endif
                                 while(msg->len>7 && !bs)
                                 {
                                   fc_delay(msg->min_sep_time);
                                   if(!(retval=send_cf(msg)))
                                   {
#ifdef ISO_TP_DEBUG
                                     Serial.print(F("Send Seq "));
                                     Serial.println(msg->seq_id);
#endif
                                     if(msg->blocksize > 0)
                                     {
#ifdef ISO_TP_DEBUG
                                       Serial.print(F("Blocksize trigger "));
                                       Serial.print(msg->seq_id %
                                                    msg->blocksize);
#endif
                                       if(!(msg->seq_id % msg->blocksize))
                                       {
                                         bs=true;
                                         msg->tp_state=ISOTP_WAIT_FC;
#ifdef ISO_TP_DEBUG
                                         Serial.println(F(" yes"));
#endif
                                       }
#ifdef ISO_TP_DEBUG
                                       else Serial.println(F(" no"));
#endif
                                     }
                                     msg->seq_id++;
				     if (msg->blocksize < 16)
                                       msg->seq_id %= 16;
                                     else
                                       msg->seq_id %= msg->blocksize;
                                     msg->Buffer+=7;
                                     msg->len-=7;
#ifdef ISO_TP_DEBUG
                                     Serial.print(F("Length      : "));
                                     Serial.println(msg->len);
#endif
                                   }
                                 }
                                 if(!bs)
                                 {
                                   fc_delay(msg->min_sep_time);
#ifdef ISO_TP_DEBUG
                                   Serial.print(F("Send last Seq "));
                                   Serial.println(msg->seq_id);
#endif
                                   retval=send_cf(msg);
                                   msg->tp_state=ISOTP_IDLE;
                                 }
                                 break;
      default                 :  break;
    }


    if(msg->tp_state==ISOTP_WAIT_FIRST_FC ||
       msg->tp_state==ISOTP_WAIT_FC)
    {
      // if(can_receive())
      uint8_t data[8];
      uint8_t len;
      uint32_t id;
      if (can_rx_func(&id, data, &len) == 0)
      {
#ifdef ISO_TP_DEBUG
        Serial.println(F("Send branch:"));
#endif
        // if(rxId==msg->rx_id)
        if(id==msg->rx_id)
        {
          retval=rcv_fc(msg);
          memset(rxBuffer,0,sizeof(rxBuffer));
#ifdef ISO_TP_DEBUG
          Serial.println(F("rxId OK!"));
#endif
        }
      }
    }
  }

  return retval;
}

uint8_t IsoTp::receive(Message_t* msg)
{
  uint8_t n_pci_type=0;
  uint32_t delta=0;

  wait_session=millis();
#ifdef ISO_TP_DEBUG
  Serial.println(F("Start receive..."));
#endif
  msg->tp_state=ISOTP_IDLE;

  while(msg->tp_state!=ISOTP_FINISHED && msg->tp_state!=ISOTP_ERROR)
  {
    delta=millis()-wait_session;
    if(delta >= TIMEOUT_SESSION)
    {
#ifdef ISO_TP_DEBUG
      Serial.print(F("ISO-TP Session timeout wait_session="));
      Serial.print(wait_session); Serial.print(F(" delta="));
      Serial.println(delta);
#endif
      return 1;
    }

    // if(can_receive())
    uint8_t data[8];
    uint8_t len;
    uint32_t id;
    if (can_rx_func(&id, data, &len) == 0)
    {
      // if(rxId==msg->rx_id)
      if(id==msg->rx_id)
      {
#ifdef ISO_TP_DEBUG
        Serial.println(F("rxId OK!"));
#endif
        memcpy(rxBuffer, data, len); // rxBuffer にデータをコピー
        n_pci_type=rxBuffer[0] & 0xF0;

        switch (n_pci_type)
        {
          case N_PCI_FC:
#ifdef ISO_TP_DEBUG
                      Serial.println(F("FC"));
#endif
                      /* tx path: fc frame */
                      rcv_fc(msg);
                      break;

          case N_PCI_SF:
#ifdef ISO_TP_DEBUG
                      Serial.println(F("SF"));
#endif
                      /* rx path: single frame */
                      rcv_sf(msg);
//		      msg->tp_state=ISOTP_FINISHED;
                      break;

          case N_PCI_FF:
#ifdef ISO_TP_DEBUG
                      Serial.println(F("FF"));
#endif
                      /* rx path: first frame */
                      rcv_ff(msg);
//		      msg->tp_state=ISOTP_WAIT_DATA;
                      break;
                      break;

          case N_PCI_CF:
#ifdef ISO_TP_DEBUG
                      Serial.println(F("CF"));
#endif
                      /* rx path: consecutive frame */
                      rcv_cf(msg);
                      break;
        }
        memset(rxBuffer,0,sizeof(rxBuffer));
      }
    }
  }
#ifdef ISO_TP_DEBUG
  Serial.println(F("ISO-TP message received:"));
  print_buffer(msg->rx_id, msg->Buffer, msg->len);
#endif

  return 0;
}
```
</details>

ISO-TP編 以上

[その４ UDS編へ](https://qiita.com/O_positive/items/4389d383230f3a24e0a1)