From df363fa1bb9f7f7f83d14380dcfb1a29ceb6ee35 Mon Sep 17 00:00:00 2001 From: Daniel Borkmann Date: Thu, 7 Apr 2011 20:31:51 +0200 Subject: [PATCH] added i2c, signals --- src/i2c.h | 350 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ src/naonet.c | 236 +++++++++++++++++++++++++++++++++++++++ src/naonet.h | 58 ++++++++++ src/signals.h | 31 ++++++ 4 files changed, 675 insertions(+) create mode 100644 src/i2c.h create mode 100644 src/signals.h diff --git a/src/i2c.h b/src/i2c.h new file mode 100644 index 00000000..f25fbb4a --- /dev/null +++ b/src/i2c.h @@ -0,0 +1,350 @@ +/* + * nao-ulib + * Copyright 2010 - 2011 Daniel Borkmann + * Subject to the GPL. + * Nao-Team HTWK, + * Faculty of Computer Science, Mathematics and Natural Sciences, + * Leipzig University of Applied Sciences (HTWK Leipzig) + */ + +/* + * i2c-dev.h - i2c-bus driver, char device interface + * + * Copyright (C) 1995-97 Simon G. Vogl + * Copyright (C) 1998-99 Frodo Looijaard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301 USA. + */ + +#ifndef I2C_H +#define I2C_H + +#include +#include + +/* + * I2C Message - used for pure i2c transaction, also from /dev interface + */ +struct i2c_msg { + __u16 addr; /* slave address */ + unsigned short flags; +#define I2C_M_TEN 0x10 /* we have a ten bit chip address */ +#define I2C_M_RD 0x01 +#define I2C_M_NOSTART 0x4000 +#define I2C_M_REV_DIR_ADDR 0x2000 +#define I2C_M_IGNORE_NAK 0x1000 +#define I2C_M_NO_RD_ACK 0x0800 + short len; /* msg length */ + char *buf; /* pointer to msg data */ +}; + +/* To determine what functionality is present */ + +#define I2C_FUNC_I2C 0x00000001 +#define I2C_FUNC_10BIT_ADDR 0x00000002 +#define I2C_FUNC_PROTOCOL_MANGLING 0x00000004 /* I2C_M_{REV_DIR_ADDR,NOSTART,..} */ +#define I2C_FUNC_SMBUS_PEC 0x00000008 +#define I2C_FUNC_SMBUS_BLOCK_PROC_CALL 0x00008000 /* SMBus 2.0f */ +#define I2C_FUNC_SMBUS_QUICK 0x00010000 +#define I2C_FUNC_SMBUS_READ_BYTE 0x00020000 +#define I2C_FUNC_SMBUS_WRITE_BYTE 0x00040000 +#define I2C_FUNC_SMBUS_READ_BYTE_DATA 0x00080000 +#define I2C_FUNC_SMBUS_WRITE_BYTE_DATA 0x00100000 +#define I2C_FUNC_SMBUS_READ_WORD_DATA 0x00200000 +#define I2C_FUNC_SMBUS_WRITE_WORD_DATA 0x00400000 +#define I2C_FUNC_SMBUS_PROC_CALL 0x00800000 +#define I2C_FUNC_SMBUS_READ_BLOCK_DATA 0x01000000 +#define I2C_FUNC_SMBUS_WRITE_BLOCK_DATA 0x02000000 +#define I2C_FUNC_SMBUS_READ_I2C_BLOCK 0x04000000 /* I2C-like block xfer */ +#define I2C_FUNC_SMBUS_WRITE_I2C_BLOCK 0x08000000 /* w/ 1-byte reg. addr. */ + +#define I2C_FUNC_SMBUS_BYTE (I2C_FUNC_SMBUS_READ_BYTE | \ + I2C_FUNC_SMBUS_WRITE_BYTE) +#define I2C_FUNC_SMBUS_BYTE_DATA (I2C_FUNC_SMBUS_READ_BYTE_DATA | \ + I2C_FUNC_SMBUS_WRITE_BYTE_DATA) +#define I2C_FUNC_SMBUS_WORD_DATA (I2C_FUNC_SMBUS_READ_WORD_DATA | \ + I2C_FUNC_SMBUS_WRITE_WORD_DATA) +#define I2C_FUNC_SMBUS_BLOCK_DATA (I2C_FUNC_SMBUS_READ_BLOCK_DATA | \ + I2C_FUNC_SMBUS_WRITE_BLOCK_DATA) +#define I2C_FUNC_SMBUS_I2C_BLOCK (I2C_FUNC_SMBUS_READ_I2C_BLOCK | \ + I2C_FUNC_SMBUS_WRITE_I2C_BLOCK) + +/* Old name, for compatibility */ +#define I2C_FUNC_SMBUS_HWPEC_CALC I2C_FUNC_SMBUS_PEC + +/* + * Data for SMBus Messages + */ +#define I2C_SMBUS_BLOCK_MAX 32 /* As specified in SMBus standard */ +#define I2C_SMBUS_I2C_BLOCK_MAX 32 /* Not specified but we use same structure */ +union i2c_smbus_data { + __u8 byte; + __u16 word; + __u8 block[I2C_SMBUS_BLOCK_MAX + 2]; /* block[0] is used for length */ + /* and one more for PEC */ +}; + +/* smbus_access read or write markers */ +#define I2C_SMBUS_READ 1 +#define I2C_SMBUS_WRITE 0 + +/* SMBus transaction types (size parameter in the above functions) + Note: these no longer correspond to the (arbitrary) PIIX4 internal codes! */ +#define I2C_SMBUS_QUICK 0 +#define I2C_SMBUS_BYTE 1 +#define I2C_SMBUS_BYTE_DATA 2 +#define I2C_SMBUS_WORD_DATA 3 +#define I2C_SMBUS_PROC_CALL 4 +#define I2C_SMBUS_BLOCK_DATA 5 +#define I2C_SMBUS_I2C_BLOCK_BROKEN 6 +#define I2C_SMBUS_BLOCK_PROC_CALL 7 /* SMBus 2.0f */ +#define I2C_SMBUS_I2C_BLOCK_DATA 8 + + +/* ----- commands for the ioctl like i2c_command call: + * note that additional calls are defined in the algorithm and hw + * dependent layers - these can be listed here, or see the + * corresponding header files. + */ + /* -> bit-adapter specific ioctls */ +#define I2C_RETRIES 0x0701 /* number of times a device address */ + /* should be polled when not */ + /* acknowledging */ +#define I2C_TIMEOUT 0x0702 /* set timeout - call with int */ + + +/* this is for i2c-dev.c */ +#define I2C_SLAVE 0x0703 /* Change slave address */ + /* Attn.: Slave address is 7 or 10 bits */ +#define I2C_SLAVE_FORCE 0x0706 /* Change slave address */ + /* Attn.: Slave address is 7 or 10 bits */ + /* This changes the address, even if it */ + /* is already taken! */ +#define I2C_TENBIT 0x0704 /* 0 for 7 bit addrs, != 0 for 10 bit */ + +#define I2C_FUNCS 0x0705 /* Get the adapter functionality */ +#define I2C_RDWR 0x0707 /* Combined R/W transfer (one stop only)*/ +#define I2C_PEC 0x0708 /* != 0 for SMBus PEC */ + +#define I2C_SMBUS 0x0720 /* SMBus-level access */ + +/* -- i2c.h -- */ + +/* Note: 10-bit addresses are NOT supported! */ + +/* This is the structure as used in the I2C_SMBUS ioctl call */ +struct i2c_smbus_ioctl_data { + char read_write; + __u8 command; + int size; + union i2c_smbus_data *data; +}; + +/* This is the structure as used in the I2C_RDWR ioctl call */ +struct i2c_rdwr_ioctl_data { + struct i2c_msg *msgs; /* pointers to i2c_msgs */ + int nmsgs; /* number of i2c_msgs */ +}; + +static inline __s32 i2c_smbus_access(int file, char read_write, __u8 command, + int size, union i2c_smbus_data *data) +{ + struct i2c_smbus_ioctl_data args; + + args.read_write = read_write; + args.command = command; + args.size = size; + args.data = data; + + return ioctl(file,I2C_SMBUS,&args); +} + + +static inline __s32 i2c_smbus_write_quick(int file, __u8 value) +{ + return i2c_smbus_access(file,value,0,I2C_SMBUS_QUICK,NULL); +} + +static inline __s32 i2c_smbus_read_byte(int file) +{ + union i2c_smbus_data data; + + if (i2c_smbus_access(file,I2C_SMBUS_READ,0,I2C_SMBUS_BYTE,&data)) + return -1; + else + return 0x0FF & data.byte; +} + +static inline __s32 i2c_smbus_write_byte(int file, __u8 value) +{ + return i2c_smbus_access(file,I2C_SMBUS_WRITE,value, + I2C_SMBUS_BYTE,NULL); +} + +static inline __s32 i2c_smbus_read_byte_data(int file, __u8 command) +{ + union i2c_smbus_data data; + + if (i2c_smbus_access(file,I2C_SMBUS_READ,command, + I2C_SMBUS_BYTE_DATA,&data)) + return -1; + else + return 0x0FF & data.byte; +} + +static inline __s32 i2c_smbus_write_byte_data(int file, __u8 command, + __u8 value) +{ + union i2c_smbus_data data; + + data.byte = value; + return i2c_smbus_access(file,I2C_SMBUS_WRITE,command, + I2C_SMBUS_BYTE_DATA, &data); +} + +static inline __s32 i2c_smbus_read_word_data(int file, __u8 command) +{ + union i2c_smbus_data data; + + if (i2c_smbus_access(file,I2C_SMBUS_READ,command, + I2C_SMBUS_WORD_DATA,&data)) + return -1; + else + return 0x0FFFF & data.word; +} + +static inline __s32 i2c_smbus_write_word_data(int file, __u8 command, + __u16 value) +{ + union i2c_smbus_data data; + + data.word = value; + return i2c_smbus_access(file,I2C_SMBUS_WRITE,command, + I2C_SMBUS_WORD_DATA, &data); +} + +static inline __s32 i2c_smbus_process_call(int file, __u8 command, __u16 value) +{ + union i2c_smbus_data data; + + data.word = value; + if (i2c_smbus_access(file,I2C_SMBUS_WRITE,command, + I2C_SMBUS_PROC_CALL,&data)) + return -1; + else + return 0x0FFFF & data.word; +} + + +/* Returns the number of read bytes */ +static inline __s32 i2c_smbus_read_block_data(int file, __u8 command, + __u8 *values) +{ + union i2c_smbus_data data; + int i; + + if (i2c_smbus_access(file,I2C_SMBUS_READ,command, + I2C_SMBUS_BLOCK_DATA,&data)) + return -1; + else { + for (i = 1; i <= data.block[0]; i++) + values[i-1] = data.block[i]; + return data.block[0]; + } +} + +static inline __s32 i2c_smbus_write_block_data(int file, __u8 command, + __u8 length, __u8 *values) +{ + union i2c_smbus_data data; + int i; + + if (length > 32) + length = 32; + + for (i = 1; i <= length; i++) + data.block[i] = values[i-1]; + data.block[0] = length; + return i2c_smbus_access(file,I2C_SMBUS_WRITE,command, + I2C_SMBUS_BLOCK_DATA, &data); +} + +/* Returns the number of read bytes */ +/* Until kernel 2.6f.22, the length is hardcoded to 32 bytes. If you + ask for less than 32 bytes, your code will only work with kernels + 2.6f.23 and later. */ +static inline __s32 i2c_smbus_read_i2c_block_data(int file, __u8 command, + __u8 length, __u8 *values) +{ + union i2c_smbus_data data; + int i; + + if (length > 32) + length = 32; + data.block[0] = length; + + if (i2c_smbus_access(file,I2C_SMBUS_READ,command, + length == 32 ? I2C_SMBUS_I2C_BLOCK_BROKEN : + I2C_SMBUS_I2C_BLOCK_DATA,&data)) + return -1; + else { + for (i = 1; i <= data.block[0]; i++) + values[i-1] = data.block[i]; + return data.block[0]; + } +} + +static inline __s32 i2c_smbus_write_i2c_block_data(int file, __u8 command, + __u8 length, __u8 *values) +{ + union i2c_smbus_data data; + int i; + + if (length > 32) + length = 32; + + for (i = 1; i <= length; i++) + data.block[i] = values[i-1]; + data.block[0] = length; + + return i2c_smbus_access(file,I2C_SMBUS_WRITE,command, + I2C_SMBUS_I2C_BLOCK_BROKEN, &data); +} + +static inline __s32 i2c_smbus_block_process_call(int file, __u8 command, + __u8 length, __u8 *values) +{ + union i2c_smbus_data data; + int i; + + if (length > 32) + length = 32; + + for (i = 1; i <= length; i++) + data.block[i] = values[i-1]; + data.block[0] = length; + + if (i2c_smbus_access(file,I2C_SMBUS_WRITE,command, + I2C_SMBUS_BLOCK_PROC_CALL,&data)) + return -1; + else { + for (i = 1; i <= data.block[0]; i++) + values[i-1] = data.block[i]; + return data.block[0]; + } +} + +#endif /* I2C_H */ diff --git a/src/naonet.c b/src/naonet.c index e69de29b..ce39f0a2 100644 --- a/src/naonet.c +++ b/src/naonet.c @@ -0,0 +1,236 @@ +/* + * nao-ulib + * Copyright 2010 - 2011 Daniel Borkmann + * Subject to the GPL. + * Nao-Team HTWK, + * Faculty of Computer Science, Mathematics and Natural Sciences, + * Leipzig University of Applied Sciences (HTWK Leipzig) + */ + +/* Needs here & there some cleanups and rewrites */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "naonet.h" + +#define NET_DEV_FILE "/dev/nao" +#define NET_DEV_MAX_SEG_SIZE 1500 + +pthread_mutex_t list_head_lock; +static naonet_block_t *list_head = NULL; + +static int __register_rx_hook(naonet_block_t **head, naonet_block_t *block) +{ + if(!block || !head) + return -EINVAL; + if(!block->rx_hook) + return -EINVAL; + + pthread_mutex_lock(&list_head_lock); + while((*head) != NULL) { + if(block->priority > (*head)->priority) + break; + head = &((*head)->next); + } + block->next = (*head); + (*head) = block; + pthread_mutex_unlock(&list_head_lock); + + return 0; +} + +static int __register_rx_hook_once(naonet_block_t **head, naonet_block_t *block) +{ + if(!block || !head) + return -EINVAL; + if(!block->rx_hook) + return -EINVAL; + + pthread_mutex_lock(&list_head_lock); + while((*head) != NULL) { + if(unlikely(block == (*head))) + return (-EEXIST); + if(block->priority > (*head)->priority) + break; + head = &((*head)->next); + } + block->next = (*head); + (*head) = block; + pthread_mutex_unlock(&list_head_lock); + + return 0; +} + +static int __unregister_rx_hook(naonet_block_t **head, naonet_block_t *block) +{ + if(!block || !head) + return -EINVAL; + + pthread_mutex_lock(&list_head_lock); + while((*head) != NULL) { + if(unlikely(block == (*head))) { + (*head) = block->next; + break; + } + head = &((*head)->next); + } + pthread_mutex_unlock(&list_head_lock); + + return 0; +} + +static int __call_rx_hook_chain(naonet_block_t **head, uint8_t type, + uint32_t src, char *message, size_t len, + int *called) +{ + int rc = BLOCK_SUCC_DONE; + naonet_block_t *block = (*head), *next_block; + + if(!head || !message || !len) + return -EINVAL; + if(called) + (*called) = 0; + + pthread_mutex_lock(&list_head_lock); + while(block) { + next_block = block->next; + if(block->type != type) + goto next; + rc = block->rx_hook(block, src, message, len); + if((rc & BLOCK_STOP_CHAIN) == BLOCK_STOP_CHAIN) + break; + if(called) + (*called)++; +next: + block = next_block; + } + pthread_mutex_unlock(&list_head_lock); + return rc; +} + +int register_rx_hook(naonet_block_t *block) +{ + return __register_rx_hook(&list_head, block); +} + +int register_rx_hook_once(naonet_block_t *block) +{ + return __register_rx_hook_once(&list_head, block); +} + +int unregister_rx_hook(naonet_block_t *block) +{ + return __unregister_rx_hook(&list_head, block); +} + +static int call_rx_hook_chain(uint8_t type, uint32_t src, char *message, + size_t len, int *called) +{ + return __call_rx_hook_chain(&list_head, type, src, message, len, + called); +} + +int send_message(char *message, size_t len, uint8_t type) +{ + int fd, rc = 0; + size_t ret; + char buf[NET_DEV_MAX_SEG_SIZE]; + + if(!message || !len || len >= NET_DEV_MAX_SEG_SIZE) + return -EINVAL; + + fd = open(NET_DEV_FILE, O_WRONLY); + if(fd < 0) { + perror("open"); + exit(EXIT_FAILURE); + } + + /* No memset in fastpath */ + memcpy(&buf[1], message, len); + buf[0] = type; + + ret = write(fd, buf, len + 1); + if(ret != (len + 1)) { + perror("write"); + rc = -EIO; + } + + close(fd); + return rc; +} + +static void *rx_thread(void *null) +{ + int fd; + size_t len; + + char buf[NET_DEV_MAX_SEG_SIZE]; + naonet_header_t *nh; + + fd = open(NET_DEV_FILE, O_RDONLY); + if(fd < 0) { + perror("open"); + pthread_exit(0); + } + + while((len = read(fd, buf, sizeof(buf))) > 0) { + nh = (naonet_header_t *) buf; + call_rx_hook_chain((uint8_t) (*(buf + sizeof(*nh))), nh->src_id, + (buf + sizeof(*nh) + 1), ntohs(nh->len) - 1, + NULL); + memset(buf, 0, sizeof(buf)); + } + + close(fd); + pthread_exit(0); +} + +uint32_t naonet_fetch_id(void) +{ + int fd; + uint32_t id = -1; + char buff[200] = {0}; + + fd = open("/proc/naonet/id", O_RDONLY); + if(fd < 0) { + perror("Cannot open /proc/naonet/id"); + return EXIT_FAILURE; + } + + if(read(fd, buff, sizeof(buff)) > 0) { + sscanf(buff, "%u", &id); + } + close(fd); + + if(id == -1) + printf("Cannot parse naonet ID from /var/log/messages!\n"); + return id; +} + +int naonet_init(pthread_t *rx_thread_p) +{ + int rc; + + if(!rx_thread_p) + return -EINVAL; + + rc = pthread_create(rx_thread_p, NULL, rx_thread, NULL); + if(rc) { + perror("pthread_create"); + exit(EXIT_FAILURE); + } + + pthread_detach((*rx_thread_p)); + return 0; +} diff --git a/src/naonet.h b/src/naonet.h index e69de29b..aba9adf1 100644 --- a/src/naonet.h +++ b/src/naonet.h @@ -0,0 +1,58 @@ +/* + * nao-ulib + * Copyright 2010 - 2011 Daniel Borkmann + * Subject to the GPL. + * Nao-Team HTWK, + * Faculty of Computer Science, Mathematics and Natural Sciences, + * Leipzig University of Applied Sciences (HTWK Leipzig) + */ + +#ifndef NAONET_H +#define NAONET_H + +#include +#include + +/* Message type codes */ +#define NAOMSG_TYPE_ALIVE 0 +#define NAOMSG_TYPE_WORLD 1 +#define NAOMSG_TYPE_CMD 2 +/* ... */ + +/* rx_hook return codes */ +#define BLOCK_SUCC_DONE 0 +#define BLOCK_STOP_CHAIN 1 + +/* Priority for naonet_block */ +typedef enum { + PRIO_VERYLOW, + PRIO_LOW, + PRIO_MEDIUM, + PRIO_HIGH, + PRIO_EXTRA, +} naonet_rx_prio_t; + +/* Network header */ +typedef struct { + uint16_t len; + uint32_t src_id; + uint16_t opt; +} naonet_header_t; + +/* Userspace callback structure */ +typedef struct naonet_block { + uint8_t type; + int (*rx_hook)(const struct naonet_block *block, uint32_t src, + const char *message, size_t len); + struct naonet_block *next; + naonet_rx_prio_t priority; +} naonet_block_t; + +extern uint32_t naonet_fetch_id(void); +extern int naonet_init(pthread_t *rx_thread_p); +extern int send_message(char *message, size_t len, uint8_t type); +extern int register_rx_hook(naonet_block_t *block); +extern int register_rx_hook_once(naonet_block_t *block); +extern int unregister_rx_hook(naonet_block_t *block); + +#endif /* NAONET_H */ diff --git a/src/signals.h b/src/signals.h new file mode 100644 index 00000000..8a4cdead --- /dev/null +++ b/src/signals.h @@ -0,0 +1,31 @@ +/* + * nao-ulib + * Copyright 2011 Daniel Borkmann + * Subject to the GPL. + * Nao-Team HTWK, + * Faculty of Computer Science, Mathematics and Natural Sciences, + * Leipzig University of Applied Sciences (HTWK Leipzig) + */ + +#ifndef SIGNALS_H +#define SIGNALS_H + +#include +#include + +static inline void register_signal(int signal, void (*handler)(int)) +{ + sigset_t block_mask; + struct sigaction saction; + + assert(handler); + + sigfillset(&block_mask); + saction.sa_handler = handler; + saction.sa_mask = block_mask; + saction.sa_flags = SA_RESTART; + + sigaction(signal, &saction, NULL); +} + +#endif /* SIGNALS_H */ -- 2.11.4.GIT