fix(class/vendor/ftdi): fix ftdi baudrate caculation

Signed-off-by: sakumisu <[email protected]>

2 files changed, 158 insertions, 30 deletions

diff --git a/class/vendor/serial/usbh_ftdi.c b/class/vendor/serial/usbh_ftdi.c
index dbf44cbc..b45f4d52 100644
--- a/class/vendor/serial/usbh_ftdi.c
+++ b/class/vendor/serial/usbh_ftdi.c
@@ -15,6 +15,25 @@ USB_NOCACHE_RAM_SECTION USB_MEM_ALIGNX uint8_t g_ftdi_buf[USB_ALIGN_UP(64, CONFI
 static struct usbh_ftdi g_ftdi_class[CONFIG_USBHOST_MAX_FTDI_CLASS];
 static uint32_t g_devinuse = 0;
 
+static const char *ftdi_chip_name[] = {
+    [SIO] = "SIO", /* the serial part of FT8U100AX */
+    [FT232A] = "FT232A",
+    [FT232B] = "FT232B",
+    [FT2232C] = "FT2232C/D",
+    [FT232R] = "FT232R",
+    [FT232H] = "FT232H",
+    [FT2232H] = "FT2232H",
+    [FT4232H] = "FT4232H",
+    [FT4232HA] = "FT4232HA",
+    [FT232HP] = "FT232HP",
+    [FT233HP] = "FT233HP",
+    [FT2232HP] = "FT2232HP",
+    [FT2233HP] = "FT2233HP",
+    [FT4232HP] = "FT4232HP",
+    [FT4233HP] = "FT4233HP",
+    [FTX] = "FT-X",
+};
+
 static struct usbh_ftdi *usbh_ftdi_class_alloc(void)
 {
     uint8_t devno;
@@ -40,33 +59,72 @@ static void usbh_ftdi_class_free(struct usbh_ftdi *ftdi_class)
     memset(ftdi_class, 0, sizeof(struct usbh_ftdi));
 }
 
-static void usbh_ftdi_caculate_baudrate(uint32_t *itdf_divisor, uint32_t actual_baudrate)
+/*
+ * Divide positive or negative dividend by positive or negative divisor
+ * and round to closest integer. Result is undefined for negative
+ * divisors if the dividend variable type is unsigned and for negative
+ * dividends if the divisor variable type is unsigned.
+ */
+#define DIV_ROUND_CLOSEST(x, divisor) (       \
+    {                                         \
+        typeof(x) __x = x;                    \
+        typeof(divisor) __d = divisor;        \
+        (((typeof(x))-1) > 0 ||               \
+         ((typeof(divisor))-1) > 0 ||         \
+         (((__x) > 0) == ((__d) > 0))) ?      \
+            (((__x) + ((__d) / 2)) / (__d)) : \
+            (((__x) - ((__d) / 2)) / (__d));  \
+    })
+
+static uint32_t ftdi_232bm_baud_base_to_divisor(uint32_t baud, int base)
 {
-#define FTDI_USB_CLK 48000000
-    int baudrate;
-    uint8_t frac[] = { 0, 8, 4, 2, 6, 10, 12, 14 };
+    static const unsigned char divfrac[8] = { 0, 3, 2, 4, 1, 5, 6, 7 };
+    uint32_t divisor;
+    /* divisor shifted 3 bits to the left */
+    int divisor3 = DIV_ROUND_CLOSEST(base, 2 * baud);
+    divisor = divisor3 >> 3;
+    divisor |= (uint32_t)divfrac[divisor3 & 0x7] << 14;
+    /* Deal with special cases for highest baud rates. */
+    if (divisor == 1) /* 1.0 */
+        divisor = 0;
+    else if (divisor == 0x4001) /* 1.5 */
+        divisor = 1;
+    return divisor;
+}
 
-    if (actual_baudrate == 2000000) {
-        *itdf_divisor = 0x01;
-    } else if (actual_baudrate == 3000000) {
-        *itdf_divisor = 0x00;
-    } else {
-        baudrate = actual_baudrate;
-        if (baudrate > 100000 && baudrate < 12000000) {
-            baudrate = (baudrate / 100000) + 100000;
-        }
-        int divisor = FTDI_USB_CLK / baudrate;
-        int frac_bits = 0;
-        for (uint8_t i = 0; i < sizeof(frac) / sizeof(frac[0]); i++) {
-            if ((divisor & 0xF) == frac[i]) {
-                frac_bits = i;
-                break;
-            }
-        }
-        divisor >>= 4;
-        divisor &= 0x3FFF;
-        *itdf_divisor = (divisor << 14) | (frac_bits << 8);
-    }
+static uint32_t ftdi_232bm_baud_to_divisor(uint32_t baud)
+{
+    return ftdi_232bm_baud_base_to_divisor(baud, 48000000);
+}
+
+static uint32_t ftdi_2232h_baud_base_to_divisor(uint32_t baud, int base)
+{
+    static const unsigned char divfrac[8] = { 0, 3, 2, 4, 1, 5, 6, 7 };
+    uint32_t divisor;
+    int divisor3;
+
+    /* hi-speed baud rate is 10-bit sampling instead of 16-bit */
+    divisor3 = DIV_ROUND_CLOSEST(8 * base, 10 * baud);
+
+    divisor = divisor3 >> 3;
+    divisor |= (uint32_t)divfrac[divisor3 & 0x7] << 14;
+    /* Deal with special cases for highest baud rates. */
+    if (divisor == 1) /* 1.0 */
+        divisor = 0;
+    else if (divisor == 0x4001) /* 1.5 */
+        divisor = 1;
+    /*
+	 * Set this bit to turn off a divide by 2.5 on baud rate generator
+	 * This enables baud rates up to 12Mbaud but cannot reach below 1200
+	 * baud with this bit set
+	 */
+    divisor |= 0x00020000;
+    return divisor;
+}
+
+static uint32_t ftdi_2232h_baud_to_divisor(uint32_t baud)
+{
+    return ftdi_2232h_baud_base_to_divisor(baud, 120000000);
 }
 
 int usbh_ftdi_reset(struct usbh_ftdi *ftdi_class)
@@ -108,7 +166,7 @@ static int usbh_ftdi_set_modem(struct usbh_ftdi *ftdi_class, uint16_t value)
 static int usbh_ftdi_set_baudrate(struct usbh_ftdi *ftdi_class, uint32_t baudrate)
 {
     struct usb_setup_packet *setup;
-    uint32_t itdf_divisor;
+    uint32_t div_value;
     uint16_t value;
     uint8_t baudrate_high;
 
@@ -117,9 +175,26 @@ static int usbh_ftdi_set_baudrate(struct usbh_ftdi *ftdi_class, uint32_t baudrat
     }
     setup = ftdi_class->hport->setup;
 
-    usbh_ftdi_caculate_baudrate(&itdf_divisor, baudrate);
-    value = itdf_divisor & 0xFFFF;
-    baudrate_high = (itdf_divisor >> 16) & 0xff;
+    switch (ftdi_class->chip_type) {
+        case FT232B:
+        case FT2232C:
+        case FT232R:
+            if (baudrate > 3000000) {
+                return -USB_ERR_INVAL;
+            }
+            div_value = ftdi_232bm_baud_to_divisor(baudrate);
+            break;
+        default:
+            if ((baudrate <= 12000000) && (baudrate >= 1200)) {
+                div_value = ftdi_2232h_baud_to_divisor(baudrate);
+            } else {
+                return -USB_ERR_INVAL;
+            }
+            break;
+    }
+
+    value = div_value & 0xFFFF;
+    baudrate_high = (div_value >> 16) & 0xff;
 
     setup->bmRequestType = USB_REQUEST_DIR_OUT | USB_REQUEST_VENDOR | USB_REQUEST_RECIPIENT_DEVICE;
     setup->bRequest = SIO_SET_BAUDRATE_REQUEST;
@@ -220,7 +295,11 @@ static int usbh_ftdi_read_modem_status(struct usbh_ftdi *ftdi_class)
 int usbh_ftdi_set_line_coding(struct usbh_ftdi *ftdi_class, struct cdc_line_coding *line_coding)
 {
     memcpy((uint8_t *)&ftdi_class->line_coding, line_coding, sizeof(struct cdc_line_coding));
-    usbh_ftdi_set_baudrate(ftdi_class, line_coding->dwDTERate);
+
+    int ret = usbh_ftdi_set_baudrate(ftdi_class, line_coding->dwDTERate);
+    if (ret < 0) {
+        return ret;
+    }
     return usbh_ftdi_set_data_format(ftdi_class, line_coding->bDataBits, line_coding->bParityType, line_coding->bCharFormat, 0);
 }
 
@@ -253,6 +332,7 @@ static int usbh_ftdi_connect(struct usbh_hubport *hport, uint8_t intf)
 {
     struct usb_endpoint_descriptor *ep_desc;
     int ret = 0;
+    uint16_t version;
 
     struct usbh_ftdi *ftdi_class = usbh_ftdi_class_alloc();
     if (ftdi_class == NULL) {
@@ -265,6 +345,34 @@ static int usbh_ftdi_connect(struct usbh_hubport *hport, uint8_t intf)
 
     hport->config.intf[intf].priv = ftdi_class;
 
+    version = hport->device_desc.bcdDevice;
+
+    switch (version) {
+        case 0x400:
+            ftdi_class->chip_type = FT232B;
+        case 0x500:
+            ftdi_class->chip_type = FT2232C;
+            break;
+        case 0x600:
+            ftdi_class->chip_type = FT232R;
+            break;
+        case 0x700:
+            ftdi_class->chip_type = FT2232H;
+            break;
+        case 0x800:
+            ftdi_class->chip_type = FT4232H;
+            break;
+        case 0x900:
+            ftdi_class->chip_type = FT232H;
+            break;
+
+        default:
+            USB_LOG_ERR("Unknown FTDI chip version:%04x\r\n", version);
+            return -USB_ERR_NOTSUPP;
+    }
+
+    USB_LOG_INFO("FTDI chip name:%s\r\n", ftdi_chip_name[ftdi_class->chip_type]);
+
     usbh_ftdi_reset(ftdi_class);
     usbh_ftdi_set_flow_ctrl(ftdi_class, SIO_DISABLE_FLOW_CTRL);
     usbh_ftdi_set_latency_timer(ftdi_class, 0x10);
diff --git a/class/vendor/serial/usbh_ftdi.h b/class/vendor/serial/usbh_ftdi.h
index faaf5214..855db8d6 100644
--- a/class/vendor/serial/usbh_ftdi.h
+++ b/class/vendor/serial/usbh_ftdi.h
@@ -39,6 +39,25 @@
 
 #define SIO_RTS_CTS_HS (0x1 << 8)
 
+enum ftdi_chip_type {
+    SIO,
+    FT232A,
+    FT232B,
+    FT2232C,
+    FT232R,
+    FT232H,
+    FT2232H,
+    FT4232H,
+    FT4232HA,
+    FT232HP,
+    FT233HP,
+    FT2232HP,
+    FT2233HP,
+    FT4232HP,
+    FT4233HP,
+    FTX,
+};
+
 struct usbh_ftdi {
     struct usbh_hubport *hport;
     struct usb_endpoint_descriptor *bulkin;  /* Bulk IN endpoint */
@@ -51,6 +70,7 @@ struct usbh_ftdi {
     uint8_t intf;
     uint8_t minor;
     uint8_t modem_status[2];
+    enum ftdi_chip_type chip_type;
 
     void *user_data;
 };