src/app/rpi-pmsm-test1/zynq_3pmdrv1_mc.c

   1 /*
   2   Communication with Zynq equipped by 3-phase
   3   motor driver connected to MicroZed on MZ_APO board.
   4   MZ_APO and 3-phase motor driver boards have been
   5   designed by Petr Porazil for PiKRON company.
   6   The Zynq VHDL design by Pavel Pisa, partially
   7   inspired by previous work done together with
   8   Martin Prudek.
   9
  10   (C) 2017 by Pavel Pisa ppisa@pikron.com
  11 */
  12
  13 #include <stdint.h>
  14 #include <unistd.h>
  15 #include <stdio.h>
  16 #include <stdlib.h>
  17 #include <string.h>
  18 #include <getopt.h>
  19 #include <fcntl.h>
  20 #include <sys/mman.h>
  21 #include <sys/ioctl.h>
  22 #include <linux/types.h>
  23 #include <linux/spi/spidev.h>
  24
  25 #include "zynq_3pmdrv1_mc.h"
  26
  27 #define Z3PMDRV1_REG_BASE_PHYS     0x43c20000
  28 #define Z3PMDRV1_REG_SIZE          0x00001000
  29
  30 #define Z3PMDRV1_REG_IRC_POS_o         0x0008
  31 #define Z3PMDRV1_REG_IRC_IDX_POS_o     0x000C
  32
  33 #define Z3PMDRV1_REG_PWM1_o            0x0010
  34 #define Z3PMDRV1_REG_PWM2_o            0x0014
  35 #define Z3PMDRV1_REG_PWM3_o            0x0018
  36
  37 #define Z3PMDRV1_REG_PWMX_VAL_m    0x00003fff
  38 #define Z3PMDRV1_REG_PWMX_EN_m     0x40000000
  39 #define Z3PMDRV1_REG_PWMX_SHDN_m   0x80000000
  40
  41 #define Z3PMDRV1_REG_ADC_SQN_STAT_o    0x0020
  42
  43 #define Z3PMDRV1_REG_ADSQST_SQN_m  0x000001ff
  44 #define Z3PMDRV1_REG_ADSQST_HAL1_m 0x00010000
  45 #define Z3PMDRV1_REG_ADSQST_HAL2_m 0x00020000
  46 #define Z3PMDRV1_REG_ADSQST_HAL3_m 0x00040000
  47 #define Z3PMDRV1_REG_ADSQST_ST1_m  0x00100000
  48 #define Z3PMDRV1_REG_ADSQST_ST2_m  0x00200000
  49 #define Z3PMDRV1_REG_ADSQST_ST3_m  0x00400000
  50 #define Z3PMDRV1_REG_ADSQST_PWST_m 0x01000000
  51
  52 #define Z3PMDRV1_REG_ADC1_o            0x0024
  53 #define Z3PMDRV1_REG_ADC2_o            0x0028
  54 #define Z3PMDRV1_REG_ADC3_o            0x002C
  55
  56 char *memdev="/dev/mem";
  57
  58 static inline
  59 uint32_t z3pmdrv1_reg_rd(z3pmdrv1_state_t *z3pmcst, unsigned reg_offs)
  60 {
  61         return *(volatile uint32_t*)((char*)z3pmcst->regs_base_virt + reg_offs);
  62 }
  63
  64 static inline
  65 void z3pmdrv1_reg_wr(z3pmdrv1_state_t *z3pmcst, unsigned reg_offs, uint32_t val)
  66 {
  67         *(volatile uint32_t*)((char*)z3pmcst->regs_base_virt + reg_offs) = val;
  68 }
  69
  70 int z3pmdrv1_transfer(z3pmdrv1_state_t *z3pmcst)
  71 {
  72         uint32_t sqn_stat;
  73         uint32_t pwm1, pwm2, pwm3;
  74         uint32_t pwm1_fl = 0;
  75         uint32_t pwm2_fl = 0;
  76         uint32_t pwm3_fl = 0;
  77         uint32_t idx;
  78
  79         pwm1 = z3pmcst->pwm[0];
  80         pwm2 = z3pmcst->pwm[1];
  81         pwm3 = z3pmcst->pwm[2];
  82
  83         if (pwm1 & Z3PMDRV1_PWM_ENABLE)
  84           pwm1_fl |= Z3PMDRV1_REG_PWMX_EN_m;
  85         if (pwm1 & Z3PMDRV1_PWM_SHUTDOWN)
  86           pwm1_fl |= Z3PMDRV1_REG_PWMX_SHDN_m;
  87         if (pwm2 & Z3PMDRV1_PWM_ENABLE)
  88           pwm2_fl |= Z3PMDRV1_REG_PWMX_EN_m;
  89         if (pwm2 & Z3PMDRV1_PWM_SHUTDOWN)
  90           pwm2_fl |= Z3PMDRV1_REG_PWMX_SHDN_m;
  91         if (pwm3 & Z3PMDRV1_PWM_ENABLE)
  92           pwm3_fl |= Z3PMDRV1_REG_PWMX_EN_m;
  93         if (pwm3 & Z3PMDRV1_PWM_SHUTDOWN)
  94           pwm3_fl |= Z3PMDRV1_REG_PWMX_SHDN_m;
  95
  96         pwm1 &= Z3PMDRV1_PWM_VALUE_m;
  97         pwm2 &= Z3PMDRV1_PWM_VALUE_m;
  98         pwm3 &= Z3PMDRV1_PWM_VALUE_m;
  99
 100         if (pwm1 > Z3PMDRV1_REG_PWMX_VAL_m)
 101                 pwm1 = Z3PMDRV1_REG_PWMX_VAL_m;
 102         if (pwm2 > Z3PMDRV1_REG_PWMX_VAL_m)
 103                 pwm2 = Z3PMDRV1_REG_PWMX_VAL_m;
 104         if (pwm3 > Z3PMDRV1_REG_PWMX_VAL_m)
 105                 pwm3 = Z3PMDRV1_REG_PWMX_VAL_m;
 106
 107         z3pmdrv1_reg_wr(z3pmcst, Z3PMDRV1_REG_PWM1_o, pwm1 | pwm1_fl);
 108         z3pmdrv1_reg_wr(z3pmcst, Z3PMDRV1_REG_PWM2_o, pwm2 | pwm2_fl);
 109         z3pmdrv1_reg_wr(z3pmcst, Z3PMDRV1_REG_PWM3_o, pwm3 | pwm3_fl);
 110
 111         z3pmcst->act_pos = z3pmdrv1_reg_rd(z3pmcst, Z3PMDRV1_REG_IRC_POS_o);
 112         idx = z3pmdrv1_reg_rd(z3pmcst, Z3PMDRV1_REG_IRC_IDX_POS_o);
 113
 114         if (idx ^ z3pmcst->index_pos) {
 115                 z3pmcst->index_occur += 1;
 116         }
 117         z3pmcst->index_pos = idx;
 118
 119         sqn_stat = z3pmdrv1_reg_rd(z3pmcst, Z3PMDRV1_REG_ADC_SQN_STAT_o);
 120         z3pmcst->curadc_sqn = sqn_stat & Z3PMDRV1_REG_ADSQST_SQN_m;
 121
 122         z3pmcst->curadc_cumsum[2] = z3pmdrv1_reg_rd(z3pmcst, Z3PMDRV1_REG_ADC3_o);
 123
 124         z3pmcst->curadc_cumsum[0] = z3pmdrv1_reg_rd(z3pmcst, Z3PMDRV1_REG_ADC2_o);
 125
 126         z3pmcst->curadc_cumsum[1] = z3pmdrv1_reg_rd(z3pmcst, Z3PMDRV1_REG_ADC1_o);
 127
 128         z3pmcst->hal_sensors =
 129                 ((sqn_stat & Z3PMDRV1_REG_ADSQST_HAL1_m)?1:0) |
 130                 ((sqn_stat & Z3PMDRV1_REG_ADSQST_HAL2_m)?2:0) |
 131                 ((sqn_stat & Z3PMDRV1_REG_ADSQST_HAL3_m)?4:0);
 132
 133         return 0;
 134 }
 135
 136
 137 /*
 138  * The support function which returns pointer to the virtual
 139  * address at which starts remapped physical region in the
 140  * process virtual memory space.
 141  */
 142 static inline
 143 void *map_phys_address(off_t region_base, size_t region_size, int opt_cached)
 144 {
 145         unsigned long mem_window_size;
 146         unsigned long pagesize;
 147         unsigned char *mm;
 148         unsigned char *mem;
 149         int fd;
 150
 151         /*
 152          * Open a device ("/dev/mem") representing physical address space
 153          * in POSIX systems
 154          */
 155         fd = open(memdev, O_RDWR | (!opt_cached? O_SYNC: 0));
 156         if (fd < 0) {
 157                 fprintf(stderr, "cannot open %s\n", memdev);
 158                 return NULL;
 159         }
 160
 161         /*
 162          * The virtual to physical address mapping translation granularity
 163          * corresponds to memory page size. This call obtains the page
 164          * size used by running operating system at given CPU architecture.
 165          * 4kB are used by Linux running on ARM, ARM64, x86 and x86_64 systems.
 166          */
 167         pagesize=sysconf(_SC_PAGESIZE);
 168
 169         /*
 170          * Extend physical region start address and size to page size boundaries
 171          * to cover complete requested region.
 172          */
 173         mem_window_size = ((region_base & (pagesize-1)) +
 174                           region_size + pagesize-1) & ~(pagesize-1);
 175
 176         /*
 177          * Map file (in our case physical memory) range at specified offset
 178          * to virtual memory ragion/area (see VMA Linux kernel structures)
 179          * of the process.
 180          */
 181         mm = mmap(NULL, mem_window_size, PROT_WRITE|PROT_READ,
 182                 MAP_SHARED, fd, region_base & ~(pagesize-1));
 183
 184         /* Report failure if the mmap is not allowed for given file or its region */
 185         if (mm == MAP_FAILED) {
 186                 return NULL;
 187         }
 188
 189         /*
 190          * Add offset in the page to the returned pointer for non-page-aligned
 191          * requests.
 192          */
 193         mem = mm + (region_base & (pagesize-1));
 194
 195         return mem;
 196 }
 197
 198 int z3pmdrv1_init(z3pmdrv1_state_t *z3pmcst)
 199 {
 200         int ret = 0;
 201         uint32_t sqn_stat;
 202
 203         if (z3pmcst->regs_base_phys == 0) {
 204                 z3pmcst->regs_base_phys = Z3PMDRV1_REG_BASE_PHYS;
 205         }
 206
 207         z3pmcst->regs_base_virt = map_phys_address(z3pmcst->regs_base_phys,
 208                                         Z3PMDRV1_REG_SIZE, 0);
 209
 210         if (z3pmcst->regs_base_virt == NULL) {
 211                 ret = -1;
 212                 return ret;
 213         }
 214
 215         sqn_stat = z3pmdrv1_reg_rd(z3pmcst, Z3PMDRV1_REG_ADC_SQN_STAT_o);
 216         z3pmcst->curadc_sqn = sqn_stat & Z3PMDRV1_REG_ADSQST_SQN_m;
 217         z3pmcst->curadc_sqn_last = z3pmcst->curadc_sqn;
 218
 219         z3pmcst->curadc_cumsum[2] = z3pmdrv1_reg_rd(z3pmcst, Z3PMDRV1_REG_ADC3_o);
 220
 221         z3pmcst->curadc_cumsum[0] = z3pmdrv1_reg_rd(z3pmcst, Z3PMDRV1_REG_ADC2_o);
 222
 223         z3pmcst->curadc_cumsum[1] = z3pmdrv1_reg_rd(z3pmcst, Z3PMDRV1_REG_ADC1_o);
 224
 225         z3pmcst->index_pos = z3pmdrv1_reg_rd(z3pmcst, Z3PMDRV1_REG_IRC_IDX_POS_o);
 226
 227         return ret;
 228 }