hi3516dv300使用ms41929控制电动变焦镜头（源码），开箱即用

#include <sys/types.h> #include <sys/socket.h> #include <netinet/in.h> #include <arpa/inet.h> #include <assert.h> #include <unistd.h> #include <stdlib.h> #include <string.h> #include <math.h> #include <time.h> #include <sys/types.h> #include <sys/stat.h> #include <fcntl.h> #include <sys/mman.h> //#include "zoom_lens.h" #include "msg_func.h" #include "cfg.h" #include "fw/comm/inc/serial.h" #include "mod/bsp/inc/bsp.h" #include "mod/codec/inc/codec.h" #include "mod/svp/inc/svp.h" #include "spi_ms41929.h" //#include "gpio.h" static const char dev[] = "/dev/mem"; #define PAGE_SIZE_MASK (~(0xfff)) #define PAGE_SIZE 0x1000 #define PROT_READ 0x1 /* Page can be read. */ #define PROT_WRITE 0x2 /* Page can be written. */ #define PROT_EXEC 0x4 /* Page can be executed. */ #define PROT_NONE 0x0 /* Page can not be accessed. */ #define PROT_GROWSDOWN 0x01000000 /* Extend change to start of \ growsdown vma (mprotect only). */ #define PROT_GROWSUP 0x02000000 /* Extend change to start of \ growsup vma (mprotect only). */ /* Sharing types (must choose one and only one of these). */ #define MAP_SHARED 0x01 /* Share changes. */ // #define MAP_FAILED (NULL) unsigned short highAndLowShiftHEX(unsigned short data) { // printf(" before shift data=%x \n",data); data = (data << 8) | (data >> 8); // 0101101110010110 data = ((data << 4) & 0xF0FF) | ((data >> 4) & 0xFF0F); // 1011010101101001 data = ((data << 2) & 0xCCCC) | ((data >> 2) & 0x3333); // 1110010110010110 data = ((data << 1) & 0xAAAA) | ((data >> 1) & 0x5555); // 1101101001101001 // printf(" after shift data=%x \n",data); return data; } static void write_reg(unsigned int Addr, unsigned int Value) { int fd = open(dev, O_RDWR | O_SYNC); if (fd < 0) { printf("open %s error!\n", dev); return; } /* addr align in page_size(4K) */ unsigned long phy_addr_in_page; unsigned long page_diff; phy_addr_in_page = Addr & PAGE_SIZE_MASK; // #define PAGE_SIZE_MASK (~(0xfff)) page_diff = Addr - phy_addr_in_page; /* size in page_size */ unsigned long size_in_page; unsigned long size = PAGE_SIZE; size_in_page = ((size + page_diff - 1) & PAGE_SIZE_MASK) + PAGE_SIZE; void *addr = mmap((void *)0, size_in_page, PROT_READ | PROT_WRITE, MAP_SHARED, fd, phy_addr_in_page); if (addr == MAP_FAILED) { printf("mmap @ 0x%x error!\n", phy_addr_in_page); close(fd); return; } unsigned int *addr_cur = (unsigned int *)(addr + page_diff); *addr_cur = Value; /* munmap */ if (munmap(addr, size_in_page) != 0) { printf("munmap failed!\n"); } close(fd); } static void write_data(unsigned int Addr, unsigned int Value) { int fd = open(dev, O_RDWR | O_SYNC); if (fd < 0) { printf("open %s error!\n", dev); return; } /* addr align in page_size(4K) */ unsigned long phy_addr_in_page; unsigned long page_diff; phy_addr_in_page = Addr & PAGE_SIZE_MASK; // #define PAGE_SIZE_MASK (~(0xfff)) page_diff = Addr - phy_addr_in_page; /* size in page_size */ unsigned long size_in_page; unsigned long size = PAGE_SIZE; size_in_page = ((size + page_diff - 1) & PAGE_SIZE_MASK) + PAGE_SIZE; void *addr = mmap((void *)0, size_in_page, PROT_READ | PROT_WRITE, MAP_SHARED, fd, phy_addr_in_page); if (addr == MAP_FAILED) { printf("mmap @ 0x%x error!\n", phy_addr_in_page); close(fd); return; } unsigned int *addr_cur = (unsigned int *)(addr + page_diff); *addr_cur = Value >> 16; *addr_cur = Value; *addr_cur = Value >> 8; /* munmap */ if (munmap(addr, size_in_page) != 0) { printf("munmap failed!\n"); } close(fd); } static unsigned int read_reg(unsigned int Addr) { unsigned int Value = 0; int fd = open(dev, O_RDWR | O_SYNC); if (fd < 0) { printf("open %s error!\n", dev); return -1; } /* addr align in page_size(4K) */ unsigned long phy_addr_in_page; unsigned long page_diff; phy_addr_in_page = Addr & PAGE_SIZE_MASK; page_diff = Addr - phy_addr_in_page; /* size in page_size */ unsigned long size_in_page; unsigned long size = PAGE_SIZE; size_in_page = ((size + page_diff - 1) & PAGE_SIZE_MASK) + PAGE_SIZE; void *addr = mmap((void *)0, size_in_page, PROT_READ | PROT_WRITE, MAP_SHARED, fd, phy_addr_in_page); if (addr == MAP_FAILED) { printf("mmap @ 0x%x error!\n", phy_addr_in_page); close(fd); return -1; } unsigned int *addr_cur = (unsigned int *)(addr + page_diff); Value = *addr_cur; /* munmap */ if (munmap(addr, size_in_page) != 0) { printf("munmap failed!\n"); } close(fd); // printf("ssp_read_reg %x %d\n", Addr, Value); return Value; } static void spi_enable(void) { write_reg(SSP_CR1, 0x12); // 0x42); } static void spi_disable(void) { write_reg(SSP_CR1, 0x0); // 0x40); } // void hi_ssp_enable(void) // { // int ret = 0; // read_reg(SSP_CR1,ret); // ret = (ret & 0xFFFD) | 0x2; // ret = ret | (0x1 << 4); /* big/little end, 1: little, 0: big */ // ret = ret | (0x1 << 15); /* wait en */ // write_reg(SSP_CR1,ret); // } // void hi_ssp_disable(void) // { // int ret = 0; // read_reg(SSP_CR1,ret); // ret = ret & (~(0x1 << 1)); // write_reg(SSP_CR1,ret); // } int hi_spi_set_frameform(unsigned char framemode, unsigned char spo, unsigned char sph, unsigned char datawidth, unsigned char bitend) { int ret = 0; ret = read_reg(SSP_CR0); if (framemode > 3) { printf("[ERROR]set frame parameter err.\n"); return -1; } ret = (ret & 0xFFCF) | (framemode << 4); if ((ret & 0x30) == 0) { if (spo > 1) { printf("[ERROR]set spo parameter err.\n"); return -1; } if (sph > 1) { printf("[ERROR]set sph parameter err.\n"); return -1; } ret = (ret & 0xFF3F) | (sph << 7) | (spo << 6); } if ((datawidth > 16) || (datawidth < 4)) { printf("[ERROR]set datawidth parameter err.\n"); return -1; } ret = (ret & 0xFFF0) | (datawidth - 1); write_reg(SSP_CR0, ret); ret = read_reg(SSP_CR1); if (bitend == 1) { ret = ret | (0x10); write_reg(SSP_CR1, ret); } ret = read_reg(SSP_CR1); printf("SSP_CR1:0x%x\n", ret); return 0; } int hi_spi_set_serialclock(unsigned char scr, unsigned char cpsdvsr) { int ret = 0; ret = read_reg(SSP_CR0); ret = (ret & 0xFF) | (scr << 8); write_reg(SSP_CR0, ret); if ((cpsdvsr & 0x1)) { printf("[ERROR]set cpsdvsr parameter err.\n"); return -1; } write_reg(SSP_CPSR, cpsdvsr); return 0; } int hi_spi_alt_mode_set(int enable) { int ret = 0; ret = read_reg(SSP_CR1); if (enable) { ret = ret & (~0x40); } else { ret = (ret & 0xFF) | 0x40; } write_reg(SSP_CR1, ret); return 0; } static int hi_spi_check_timeout(void) { unsigned int value = 0; unsigned int tmp = 0; while (1) { value = read_reg(SSP_SR); if ((value & SPI_SR_TFE) && (!(value & SPI_SR_BSY))) { break; } if (tmp++ > MAX_WAIT) { printf("spi transfer wait timeout!\n"); return -1; } usleep(1); } return 0; } unsigned int spi_ms41929_write(unsigned char addr, unsigned short data) { int ret = 0; unsigned int spicr0_value = 0; unsigned int receive_data = 0; unsigned int send_data = 0; // unsigned int return_data=0; // while (1) // clear recive data fifo // { // spicr0_value = read_reg(SSP_SR); // if (sp