rooty/PiBits
1
0
Fork 0
mirror of https://github.com/richardghirst/PiBits.git synced 2025-02-20 14:54:14 +01:00
Code Issues Releases Activity

Rework to process commands as they are issued, rather than on close()

Browse Source
This commit is contained in:
Richard Hirst 2012-11-23 22:31:42 +00:00
parent 4dfdf11b7b
commit fe63b9899e
3 changed files with 108 additions and 111 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ServoBlaster/Makefile
View File

@ -1,5 +1,7 @@
KERNEL_TREE := /home/richard/github/linux KERNEL_TREE := /home/richard/raspberrypi/linux
INSTALL_PATH := /lib/modules/$(shell /bin/uname -r)/kernel/drivers/misc/servoblaster INSTALL_PATH := /lib/modules/$(shell /bin/uname -r)/kernel/drivers/misc/servoblaster
#CROSS_OPTS := CROSS_COMPILE=/usr/bin/arm-linux-gnueabi- ARCH=arm
CROSS_OPTS :=
.PHONY: all install install_autostart .PHONY: all install install_autostart
all: servoblaster.ko all: servoblaster.ko
@ -7,7 +9,7 @@ all: servoblaster.ko
servoblaster.ko: servoblaster.c servoblaster.h servoblaster.ko: servoblaster.c servoblaster.h
@[ -d ${KERNEL_TREE} ] || { echo "Edit Makefile to set KERNEL_TREE to point at your kernel"; exit 1; } @[ -d ${KERNEL_TREE} ] || { echo "Edit Makefile to set KERNEL_TREE to point at your kernel"; exit 1; }
@[ -e ${KERNEL_TREE}/Module.symvers ] || { echo "KERNEL_TREE/Module.symvers does not exist, you need to configure and compile your kernel"; exit 1; } @[ -e ${KERNEL_TREE}/Module.symvers ] || { echo "KERNEL_TREE/Module.symvers does not exist, you need to configure and compile your kernel"; exit 1; }
make -C ${KERNEL_TREE} ARCH=arm M=$(PWD) modules make -C ${KERNEL_TREE} ${CROSS_OPTS} M=$(PWD) modules
servodemo: servodemo.c servoblaster.h servodemo: servodemo.c servoblaster.h
gcc -Wall -g -O2 -o servodemo servodemo.c -Wl,--export-dynamic `pkg-config --cflags gtk+-3.0 gmodule-export-2.0` `pkg-config --libs gtk+-3.0 gmodule-export-2.0` gcc -Wall -g -O2 -o servodemo servodemo.c -Wl,--export-dynamic `pkg-config --cflags gtk+-3.0 gmodule-export-2.0` `pkg-config --libs gtk+-3.0 gmodule-export-2.0`
@ -30,6 +32,6 @@ install_autostart: install
@echo " modprobe -r servoblaster" @echo " modprobe -r servoblaster"
clean: clean:
make -C ${KERNEL_TREE} ARCH=arm CROSS_COMPILE=/usr/bin/arm-linux-gnueabi- M=$(PWD) clean make -C ${KERNEL_TREE} ${CROSS_OPTS} M=$(PWD) clean
rm -f servodemo rm -f servodemo

211
ServoBlaster/servoblaster.c
View File

@ -122,11 +122,30 @@ static uint8_t servo2gpio[] = {
}; };
#define NUM_SERVOS (sizeof(servo2gpio)/sizeof(servo2gpio[0])) #define NUM_SERVOS (sizeof(servo2gpio)/sizeof(servo2gpio[0]))
// This struct is used to store all temporary data associated with a given
// open() of /dev/servoblaster
struct private_data
{
// Stores the return string for a read of /dev/servoblaster
// Allowing 10 chars per line.
int rd_len;
char rd_data[NUM_SERVOS * 10];
// Stores partial command strings between calls to write(), in case
// someone uses multiple write() calls to issue a single command.
int partial_len;
char partial_cmd[10];
// If we get bad data on a write() we reject all subsequent writes
// until the process closes and reopens the device.
int reject_writes;
};
// Structure of our control data, stored in a 4K page, and accessed by dma controller // Structure of our control data, stored in a 4K page, and accessed by dma controller
struct ctldata_s { struct ctldata_s {
struct bcm2708_dma_cb cb[NUM_SERVOS * 4]; // gpio-hi, delay, gpio-lo, delay, for each servo output struct bcm2708_dma_cb cb[NUM_SERVOS * 4]; // gpio-hi, delay, gpio-lo, delay, for each servo output
uint32_t gpiodata[NUM_SERVOS]; // set-pin, clear-pin values, per servo output uint32_t gpiodata[NUM_SERVOS]; // set-pin, clear-pin values, per servo output
uint32_t pwmdata; // the word we write to the pwm fifo uint32_t pwmdata; // the word we write to the pwm fifo
}; };
static struct ctldata_s *ctl; static struct ctldata_s *ctl;
@ -142,6 +161,11 @@ static int my_major;
static int cycle_ticks = 2000; static int cycle_ticks = 2000;
static int tick_scale = 6; static int tick_scale = 6;
// This records the written count values so we can display them on a read()
// call. Cannot derive data directly from DMA control blocks as current
// algorithm has a special case for a count of zero.
static int servo_pos[NUM_SERVOS] = { 0 };
// Wait until we're not processing the given servo (actually wait until // Wait until we're not processing the given servo (actually wait until
// we are not processing the low period of the previous servo, or the // we are not processing the low period of the previous servo, or the
// high period of this one). // high period of this one).
@ -315,105 +339,54 @@ void cleanup_module(void)
unregister_chrdev_region(devno, 1); unregister_chrdev_region(devno, 1);
} }
// This struct is used to store all temporary data required per process
// which reads/writes the device file.
struct process_data
{
// Stores the /dev/servoblaster content for a given user process.
// Allowing 10 chars per line.
int ret_idx;
char ret_data[NUM_SERVOS * 10];
// Stores up to NUM_SERVOS user commands (of up to 10 chars) per user process.
// e.g. "1=60\n2=45\n3=180\n"
int cmd_idx;
char cmd_str[NUM_SERVOS * 10];
};
// kmalloc the temporary data required for each user: // kmalloc the temporary data required for each user:
static int dev_open(struct inode *inod,struct file *fil) static int dev_open(struct inode *inod, struct file *fil)
{ {
fil->private_data = kmalloc( sizeof(struct process_data), GFP_KERNEL ); fil->private_data = kmalloc(sizeof(struct private_data), GFP_KERNEL);
if (0 == fil->private_data) if (0 == fil->private_data)
{ {
printk(KERN_WARNING "ServoBlaster: Failed to allocate user data\n"); printk(KERN_WARNING "ServoBlaster: Failed to allocate user data\n");
return -ENOMEM; return -ENOMEM;
} }
memset(fil->private_data, 0, sizeof(struct process_data)); memset(fil->private_data, 0, sizeof(struct private_data));
return 0; return 0;
} }
// This records the written count values. Cannot derive data directly from DMA static ssize_t dev_read(struct file *filp, char *buf, size_t count, loff_t *f_pos)
// control blocks as current algorithm has a special case for a count of zero.
static int written_data[NUM_SERVOS] = { 0 };
static ssize_t dev_read(struct file *filp,char *buf,size_t count,loff_t *f_pos)
{ {
ssize_t bytesPrinted = 0; ssize_t ret = 0;
struct process_data* const pdata = filp->private_data; struct private_data* const pdata = filp->private_data;
// Only proceed if we have private data, else return EOF. // Only proceed if we have private data, else return EOF.
if (0 != pdata) if (pdata) {
{ if (0 == *f_pos) {
int servo; int servo;
int* const idx = &(pdata->ret_idx); char *p = pdata->rd_data, *end = p + sizeof(pdata->rd_data);
char* const returnedData = pdata->ret_data;
if (0 == *f_pos)
{
// Get fresh data // Get fresh data
for (servo=0, *idx=0; servo < NUM_SERVOS; ++servo) for (servo = 0; servo < NUM_SERVOS; ++servo) {
{ p += snprintf(p, end - p, "%i=%i\n", servo,
*idx += snprintf(returnedData+*idx, sizeof(pdata->ret_data)-*idx, servo_pos[servo]);
"%i=%i\n",
servo,
written_data[servo]
);
} }
pdata->rd_len = end - p;
} }
if (*f_pos >= *idx) if (*f_pos < pdata->rd_len) {
{ if (count > pdata->rd_len - *f_pos)
//EOF count = pdata->rd_len - *f_pos;
bytesPrinted=0; if (copy_to_user(buf, pdata->rd_data + *f_pos, count))
}
else if ( (*f_pos + count) < *idx )
{
// Sufficient data to fulfil request
if (copy_to_user(buf,returnedData+*f_pos,count)) {
return -EFAULT; return -EFAULT;
} *f_pos += count;
*f_pos+=count; ret = count;
bytesPrinted=count;
}
else
{
// Return all the data we have
const int nBytes = *idx-*f_pos;
if (copy_to_user(buf,returnedData+*f_pos, nBytes)) {
return -EFAULT;
}
*f_pos+=nBytes;
bytesPrinted=nBytes;
} }
} }
return bytesPrinted;
return ret;
} }
// This function takes a single null terminated command string of this static int set_servo(int servo, int cnt)
// sort of format;
// 2=130
// where in this case, servo 2 is given a high duration of 130.
ssize_t process_command_string(const char str[])
{ {
int servo;
int cnt;
int n;
n = sscanf(str, "%d=%d", &servo, &cnt);
if (n != 2) {
printk(KERN_WARNING "ServoBlaster: Failed to parse command (n=%d)\n", n);
return -EINVAL;
}
if (servo < 0 || servo >= NUM_SERVOS) { if (servo < 0 || servo >= NUM_SERVOS) {
printk(KERN_WARNING "ServoBlaster: Bad servo number %d\n", servo); printk(KERN_WARNING "ServoBlaster: Bad servo number %d\n", servo);
return -EINVAL; return -EINVAL;
@ -435,57 +408,79 @@ ssize_t process_command_string(const char str[])
ctl->cb[servo*4+1].length = cnt * sizeof(uint32_t); ctl->cb[servo*4+1].length = cnt * sizeof(uint32_t);
ctl->cb[servo*4+3].length = (cycle_ticks / 8 - cnt) * sizeof(uint32_t); ctl->cb[servo*4+3].length = (cycle_ticks / 8 - cnt) * sizeof(uint32_t);
} }
written_data[servo] = cnt; // Record data for use by dev_read servo_pos[servo] = cnt; // Record data for use by dev_read
local_irq_enable(); local_irq_enable();
return 0; return 0;
} }
// Read user data into buffer until full. static ssize_t dev_write(struct file *filp,const char *user_buf,size_t count,loff_t *f_pos)
static ssize_t dev_write(struct file *filp,const char *buf,size_t count,loff_t *f_pos)
{ {
struct process_data* const pdata = filp->private_data; struct private_data* const pdata = filp->private_data;
if (0 == pdata) return 0; char buf[128], *p = buf, nl;
int len = pdata->partial_len;
{ if (0 == pdata)
static const int max_idx = sizeof(pdata->cmd_str) - 1; return -EFAULT;
int* const idx = &(pdata->cmd_idx); if (pdata->reject_writes)
return -EINVAL;
memcpy(buf, pdata->partial_cmd, len);
pdata->partial_len = 0;
if (count > sizeof(buf) - len - 1)
count = sizeof(buf) - len - 1;
if (copy_from_user(buf+len, user_buf, count))
return -EFAULT;
len += count;
buf[len] = '\0';
while (p < buf+len) {
int servo, cnt, res;
if ((*idx+count) > max_idx) count = max_idx-*idx; if (strchr(p, '\n')) {
if (copy_from_user(pdata->cmd_str + *idx, buf, count)) { if (sscanf(p, "%d=%d%c", &servo, &cnt, &nl) != 3 ||
return -EFAULT; nl != '\n') {
printk(KERN_WARNING "ServoBlaster: Bad data format\n");
pdata->reject_writes = 1;
return -EINVAL;
}
res = set_servo(servo, cnt);
if (res < 0) {
pdata->reject_writes = 1;
return res;
}
p = strchr(p, '\n') + 1;
}
else if (buf+len - p > 10) {
printk(KERN_WARNING "ServoBlaster: Bad data format\n");
pdata->reject_writes = 1;
return -EINVAL;
}
else {
// assume more data is coming...
break;
} }
*idx+=count;
} }
pdata->partial_len = buf+len - p;
memcpy(pdata->partial_cmd, p, pdata->partial_len);
return count; return count;
} }
// dev_close separates the user input (delimited by \n) into strings for passing
// to process_command_string() then frees up all process data
static int dev_close(struct inode *inod,struct file *fil) static int dev_close(struct inode *inod,struct file *fil)
{ {
struct process_data* const pdata = fil->private_data; struct private_data* const pdata = fil->private_data;
if (0 != pdata) int ret = 0;
{
static const int max_idx = sizeof(pdata->cmd_str) - 1;
char* cmd_str = pdata->cmd_str;
char* command;
cmd_str[max_idx] = 0; // Ensure command string is null terminated.
// Execute all commands. if (pdata) {
command = strsep(&cmd_str, "\n"); if (pdata->partial_len) {
while( NULL != command ) { printk(KERN_WARNING "ServoBlaster: partial command "
if (*command != 0) { "pending on close()\n");
printk(KERN_DEBUG "ServoBlaster: Command %s\n", command); ret = -EIO;
(void)process_command_string(command);
}
command = strsep(&cmd_str, "\n");
} }
// Free process data. // Free process data.
kfree(pdata); kfree(pdata);
} }
return 0;
return ret;
} }
static long dev_ioctl(struct file *file, unsigned int cmd, unsigned long arg) static long dev_ioctl(struct file *file, unsigned int cmd, unsigned long arg)

BIN
ServoBlaster/servoblaster.ko
View File

Binary file not shown.