From 5a6f6129ca3d5650e9cf8d19c23a43c5b5152cf0 Mon Sep 17 00:00:00 2001 From: Pavel Pisa Date: Thu, 2 Mar 2017 12:54:40 +0100 Subject: [PATCH] RPi PXMC Test: 3-phase motor driver connected to MicroZed on MZ_APO board support. Signed-off-by: Pavel Pisa --- src/app/rpi-pmsm-test1/zynq_3pmdrv1_mc.c | 228 +++++++++++++++++++++++ src/app/rpi-pmsm-test1/zynq_3pmdrv1_mc.h | 33 ++++ 2 files changed, 261 insertions(+) create mode 100644 src/app/rpi-pmsm-test1/zynq_3pmdrv1_mc.c create mode 100644 src/app/rpi-pmsm-test1/zynq_3pmdrv1_mc.h diff --git a/src/app/rpi-pmsm-test1/zynq_3pmdrv1_mc.c b/src/app/rpi-pmsm-test1/zynq_3pmdrv1_mc.c new file mode 100644 index 0000000..4c55186 --- /dev/null +++ b/src/app/rpi-pmsm-test1/zynq_3pmdrv1_mc.c @@ -0,0 +1,228 @@ +/* + Communication with Zynq equipped by 3-phase + motor driver connected to MicroZed on MZ_APO board. + MZ_APO and 3-phase motor driver boards have been + designed by Petr Porazil for PiKRON company. + The Zynq VHDL design by Pavel Pisa, partially + inspired by previous work done together with + Martin Prudek. + + (C) 2017 by Pavel Pisa ppisa@pikron.com +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "zynq_3pmdrv1_mc.h" + +#define Z3PMDRV1_REG_BASE_PHYS 0x43c20000 +#define Z3PMDRV1_REG_SIZE 0x00001000 + +#define Z3PMDRV1_REG_IRC_POS_o 0x0008 +#define Z3PMDRV1_REG_IRC_IDX_POS_o 0x000C + +#define Z3PMDRV1_REG_PWM1_o 0x0010 +#define Z3PMDRV1_REG_PWM2_o 0x0014 +#define Z3PMDRV1_REG_PWM3_o 0x0018 + +#define Z3PMDRV1_REG_PWMX_VAL_m 0x00003fff +#define Z3PMDRV1_REG_PWMX_EN_m 0x40000000 +#define Z3PMDRV1_REG_PWMX_SHDN_m 0x80000000 + +#define Z3PMDRV1_REG_ADC_SQN_STAT_o 0x0020 + +#define Z3PMDRV1_REG_ADSQST_SQN_m 0x000001ff +#define Z3PMDRV1_REG_ADSQST_HAL1_m 0x00010000 +#define Z3PMDRV1_REG_ADSQST_HAL2_m 0x00020000 +#define Z3PMDRV1_REG_ADSQST_HAL3_m 0x00040000 +#define Z3PMDRV1_REG_ADSQST_ST1_m 0x00100000 +#define Z3PMDRV1_REG_ADSQST_ST2_m 0x00200000 +#define Z3PMDRV1_REG_ADSQST_ST3_m 0x00400000 +#define Z3PMDRV1_REG_ADSQST_PWST_m 0x01000000 + +#define Z3PMDRV1_REG_ADC1_o 0x0024 +#define Z3PMDRV1_REG_ADC2_o 0x0028 +#define Z3PMDRV1_REG_ADC3_o 0x002C + +char *memdev="/dev/mem"; + +static inline +uint32_t z3pmdrv1_reg_rd(z3pmdrv1_state_t *z3pmcst, unsigned reg_offs) +{ + return *(volatile uint32_t*)((char*)z3pmcst->regs_base_virt + reg_offs); +} + +static inline +void z3pmdrv1_reg_wr(z3pmdrv1_state_t *z3pmcst, unsigned reg_offs, uint32_t val) +{ + *(volatile uint32_t*)((char*)z3pmcst->regs_base_virt + reg_offs) = val; +} + +int z3pmdrv1_transfer(z3pmdrv1_state_t *z3pmcst) +{ + uint32_t sqn_stat; + uint32_t pwm1, pwm2, pwm3; + uint32_t pwm1_fl = 0; + uint32_t pwm2_fl = 0; + uint32_t pwm3_fl = 0; + uint32_t idx; + + pwm1 = z3pmcst->pwm[0]; + pwm2 = z3pmcst->pwm[1]; + pwm3 = z3pmcst->pwm[2]; + + if (pwm1 & Z3PMDRV1_PWM_ENABLE) + pwm1_fl |= Z3PMDRV1_REG_PWMX_EN_m; + if (pwm1 & Z3PMDRV1_PWM_SHUTDOWN) + pwm1_fl |= Z3PMDRV1_REG_PWMX_SHDN_m; + if (pwm2 & Z3PMDRV1_PWM_ENABLE) + pwm2_fl |= Z3PMDRV1_REG_PWMX_EN_m; + if (pwm2 & Z3PMDRV1_PWM_SHUTDOWN) + pwm2_fl |= Z3PMDRV1_REG_PWMX_SHDN_m; + if (pwm3 & Z3PMDRV1_PWM_ENABLE) + pwm3_fl |= Z3PMDRV1_REG_PWMX_EN_m; + if (pwm3 & Z3PMDRV1_PWM_SHUTDOWN) + pwm3_fl |= Z3PMDRV1_REG_PWMX_SHDN_m; + + pwm1 &= Z3PMDRV1_PWM_VALUE_m; + pwm2 &= Z3PMDRV1_PWM_VALUE_m; + pwm3 &= Z3PMDRV1_PWM_VALUE_m; + + if (pwm1 > Z3PMDRV1_REG_PWMX_VAL_m) + pwm1 = Z3PMDRV1_REG_PWMX_VAL_m; + if (pwm2 > Z3PMDRV1_REG_PWMX_VAL_m) + pwm2 = Z3PMDRV1_REG_PWMX_VAL_m; + if (pwm3 > Z3PMDRV1_REG_PWMX_VAL_m) + pwm3 = Z3PMDRV1_REG_PWMX_VAL_m; + + z3pmdrv1_reg_wr(z3pmcst, Z3PMDRV1_REG_PWM1_o, pwm1 | pwm1_fl); + z3pmdrv1_reg_wr(z3pmcst, Z3PMDRV1_REG_PWM2_o, pwm2 | pwm2_fl); + z3pmdrv1_reg_wr(z3pmcst, Z3PMDRV1_REG_PWM3_o, pwm3 | pwm3_fl); + + z3pmcst->act_pos = z3pmdrv1_reg_rd(z3pmcst, Z3PMDRV1_REG_IRC_POS_o); + idx = z3pmdrv1_reg_rd(z3pmcst, Z3PMDRV1_REG_IRC_IDX_POS_o); + + if (idx ^ z3pmcst->index_pos) { + z3pmcst->index_occur += 1; + } + z3pmcst->index_pos = idx; + + sqn_stat = z3pmdrv1_reg_rd(z3pmcst, Z3PMDRV1_REG_ADC_SQN_STAT_o); + z3pmcst->curadc_sqn = sqn_stat & Z3PMDRV1_REG_ADSQST_SQN_m; + + z3pmcst->curadc_cumsum[2] = z3pmdrv1_reg_rd(z3pmcst, Z3PMDRV1_REG_ADC3_o); + + z3pmcst->curadc_cumsum[0] = z3pmdrv1_reg_rd(z3pmcst, Z3PMDRV1_REG_ADC2_o); + + z3pmcst->curadc_cumsum[1] = z3pmdrv1_reg_rd(z3pmcst, Z3PMDRV1_REG_ADC1_o); + + z3pmcst->hal_sensors = + ((sqn_stat & Z3PMDRV1_REG_ADSQST_HAL1_m)?1:0) | + ((sqn_stat & Z3PMDRV1_REG_ADSQST_HAL2_m)?2:0) | + ((sqn_stat & Z3PMDRV1_REG_ADSQST_HAL3_m)?4:0); + + return 0; +} + + +/* + * The support function which returns pointer to the virtual + * address at which starts remapped physical region in the + * process virtual memory space. + */ +static inline +void *map_phys_address(off_t region_base, size_t region_size, int opt_cached) +{ + unsigned long mem_window_size; + unsigned long pagesize; + unsigned char *mm; + unsigned char *mem; + int fd; + + /* + * Open a device ("/dev/mem") representing physical address space + * in POSIX systems + */ + fd = open(memdev, O_RDWR | (!opt_cached? O_SYNC: 0)); + if (fd < 0) { + fprintf(stderr, "cannot open %s\n", memdev); + return NULL; + } + + /* + * The virtual to physical address mapping translation granularity + * corresponds to memory page size. This call obtains the page + * size used by running operating system at given CPU architecture. + * 4kB are used by Linux running on ARM, ARM64, x86 and x86_64 systems. + */ + pagesize=sysconf(_SC_PAGESIZE); + + /* + * Extend physical region start address and size to page size boundaries + * to cover complete requested region. + */ + mem_window_size = ((region_base & (pagesize-1)) + + region_size + pagesize-1) & ~(pagesize-1); + + /* + * Map file (in our case physical memory) range at specified offset + * to virtual memory ragion/area (see VMA Linux kernel structures) + * of the process. + */ + mm = mmap(NULL, mem_window_size, PROT_WRITE|PROT_READ, + MAP_SHARED, fd, region_base & ~(pagesize-1)); + + /* Report failure if the mmap is not allowed for given file or its region */ + if (mm == MAP_FAILED) { + return NULL; + } + + /* + * Add offset in the page to the returned pointer for non-page-aligned + * requests. + */ + mem = mm + (region_base & (pagesize-1)); + + return mem; +} + +int z3pmdrv1_init(z3pmdrv1_state_t *z3pmcst) +{ + int ret = 0; + uint32_t sqn_stat; + + if (z3pmcst->regs_base_phys == 0) { + z3pmcst->regs_base_phys = Z3PMDRV1_REG_BASE_PHYS; + } + + z3pmcst->regs_base_virt = map_phys_address(z3pmcst->regs_base_phys, + Z3PMDRV1_REG_SIZE, 0); + + if (z3pmcst->regs_base_virt == NULL) { + ret = -1; + return ret; + } + + sqn_stat = z3pmdrv1_reg_rd(z3pmcst, Z3PMDRV1_REG_ADC_SQN_STAT_o); + z3pmcst->curadc_sqn = sqn_stat & Z3PMDRV1_REG_ADSQST_SQN_m; + z3pmcst->curadc_sqn_last = z3pmcst->curadc_sqn; + + z3pmcst->curadc_cumsum[2] = z3pmdrv1_reg_rd(z3pmcst, Z3PMDRV1_REG_ADC3_o); + + z3pmcst->curadc_cumsum[0] = z3pmdrv1_reg_rd(z3pmcst, Z3PMDRV1_REG_ADC2_o); + + z3pmcst->curadc_cumsum[1] = z3pmdrv1_reg_rd(z3pmcst, Z3PMDRV1_REG_ADC1_o); + + z3pmcst->index_pos = z3pmdrv1_reg_rd(z3pmcst, Z3PMDRV1_REG_IRC_IDX_POS_o); + + return ret; +} diff --git a/src/app/rpi-pmsm-test1/zynq_3pmdrv1_mc.h b/src/app/rpi-pmsm-test1/zynq_3pmdrv1_mc.h new file mode 100644 index 0000000..82d2e80 --- /dev/null +++ b/src/app/rpi-pmsm-test1/zynq_3pmdrv1_mc.h @@ -0,0 +1,33 @@ +#ifndef _ZYNQ_3PMDRV1_MC_H +#define _ZYNQ_3PMDRV1_MC_H + +#include + +#define Z3PMDRV1_CHAN_COUNT 3 + +#define Z3PMDRV1_PWM_VALUE_m 0x0ffff +#define Z3PMDRV1_PWM_ENABLE 0x10000 +#define Z3PMDRV1_PWM_SHUTDOWN 0x20000 + +typedef struct z3pmdrv1_state_t { + uintptr_t regs_base_phys; + void *regs_base_virt; + uint32_t pwm[Z3PMDRV1_CHAN_COUNT]; + uint32_t act_pos; + uint32_t index_pos; + uint32_t index_occur; + uint32_t pos_offset; + int32_t curadc_val[Z3PMDRV1_CHAN_COUNT]; + int32_t curadc_offs[Z3PMDRV1_CHAN_COUNT]; + uint8_t hal_sensors; + uint16_t curadc_sqn; + uint16_t curadc_sqn_last; + uint32_t curadc_cumsum[Z3PMDRV1_CHAN_COUNT]; + uint32_t curadc_cumsum_last[Z3PMDRV1_CHAN_COUNT]; +} z3pmdrv1_state_t; + +int z3pmdrv1_init(z3pmdrv1_state_t *z3pmcst); + +int z3pmdrv1_transfer(z3pmdrv1_state_t *z3pmcst); + +#endif /*_ZYNQ_3PMDRV1_MC_H*/ -- 2.39.2