RPi PXMC Test: add evaluation and extension of index position to 32 bits.

[fpga/rpi-motor-control-pxmc.git] / src / app / rpi-pmsm-test1 / rpi_spi.c

diff --git a/src/app/rpi-pmsm-test1/rpi_spi.c b/src/app/rpi-pmsm-test1/rpi_spi.c
index 1976449a47cc4b2d7115192edc51b7b283a61fae..5e93cc1f9af72f46ec632a86c42f05b9095d212b 100644 (file)
--- a/src/app/rpi-pmsm-test1/rpi_spi.c
+++ b/src/app/rpi-pmsm-test1/rpi_spi.c
@@ -21,6 +21,9 @@
 
 #include "pxmc_spimc.h"
 
+#define SPIMC_INDEX_BITS 12
+#define SPIMC_INDEX_MASK ((1 << SPIMC_INDEX_BITS) - 1)
+
 static uint8_t spimc_mode = 0;
 static uint8_t spimc_bits = 8;
 static uint32_t spimc_speed = 500000;
@@ -37,11 +40,12 @@ int spimc_transfer(spimc_state_t *spimcst)
        uint8_t *tx = spimcst->tx_buf;
        uint8_t *rx = spimcst->rx_buf;
        int ret;
-       uint16_t tmp;
        uint32_t pwm1, pwm2, pwm3;
+       uint32_t idx;
+       int32_t  idxdiff;
 
-        memset(tx, 0, SPIMC_TRANSFER_SIZE);
-        memset(rx, 0, SPIMC_TRANSFER_SIZE);
+       memset(tx, 0, SPIMC_TRANSFER_SIZE);
+       memset(rx, 0, SPIMC_TRANSFER_SIZE);
 
        /*Data format:
         * rx[0] - bity 127 downto 120 the first income bit..127
@@ -69,12 +73,9 @@ int spimc_transfer(spimc_state_t *spimcst)
         * bit 122 - shutdown2
         * bit 121 - shutdown3
         *      .
-        *      .
-        *      .
-        * bits 66 .. 56 - match PWM1
-        * bits 55 .. 45 - match PWM2
-        * bit 11,12 - Unused
-        * bits 42 .. 32  - match PWM3
+        * bits 47 .. 32 - match PWM1
+        * bits 31 .. 16 - match PWM2
+        * bits 15 .. 0  - match PWM3
         */
 
        pwm1 = spimcst->pwm[0];
@@ -105,18 +106,16 @@ int spimc_transfer(spimc_state_t *spimcst)
        if (pwm3 > 2047) pwm3 = 2047;
 
        /*pwm1*/
-       tx[7]=(tx[7] & 0xF8) | (0x07 & ((uint8_t*)&pwm1)[1]); /*MSB*/
-       tx[8]=((uint8_t*)&pwm1)[0]; /*LSB*/
+       tx[10] = pwm1 >> 8;   /*MSB*/
+       tx[11] = pwm1 & 0xff; /*LSB*/
 
        /*pwm2*/
-       tmp=pwm2;
-       tmp<<=5;
-       tx[9]=((uint8_t*)&tmp)[1]; /*MSB*/
-       tx[10]=(tx[10] & 0x1F) | (0xE0 & ((uint8_t*)&tmp)[0]); /*LSB*/
+       tx[12] = pwm2 >> 8;   /*MSB*/
+       tx[13] = pwm2 & 0xff; /*LSB*/
 
        /*pwm3*/
-       tx[10]=(tx[10] & 0xF8) | (0x07 & ((uint8_t*)&pwm3)[1]); /*MSB*/
-       tx[11]=((uint8_t*)&pwm3)[0]; /*LSB*/
+       tx[14] = pwm3 >> 8;   /*MSB*/
+       tx[15] = pwm3 & 0xff; /*LSB*/
 
        struct spi_ioc_transfer tr = {
                .tx_buf = (uintptr_t)tx,
@@ -170,10 +169,19 @@ int spimc_transfer(spimc_state_t *spimcst)
         *      92..88 in rx[4] last 5 bits (from left)
         *      87..81 in rx[5] first 7 bits (from left)
         */
-       spimcst->index_pos = 0x1F & rx[4];
-       spimcst->index_pos <<= 8;
-       spimcst->index_pos |= 0xFE & rx[5];
-       spimcst->index_pos >>= 1;
+       idx = 0x1F & rx[4];
+       idx <<= 8;
+       idx |= 0xFE & rx[5];
+       idx >>= 1;
+
+       if ((idx ^ spimcst->index_pos) & SPIMC_INDEX_MASK) {
+               idxdiff = (idx - spimcst->act_pos +
+                          (1 << (SPIMC_INDEX_BITS - 1))) & SPIMC_INDEX_MASK;
+               idxdiff -= 1 << (SPIMC_INDEX_BITS - 1);
+               idx = spimcst->act_pos + idxdiff;
+               spimcst->index_pos = idx;
+               spimcst->index_occur += 1;
+       }
 
        /* current measurments count
         * bits 80 downto 72